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ABSTRACT 


A  tactical  missile  with  mid-course  requires  the  use  of  an  Inertial 
Navigation  System  (INS).  Steady-state  Kalman  Filters  (SKF)  used  as 
estimators  have  been  proposed  for  use  in  a  Strapdown  INS  that  is  con- 
sidered to  be  cheaper  and  easier  to  implement  than  a  gimbaled  INS. 

This  thesis  further  investigates  the  sensitivity  of  the  SKF  to  in- 
accuracies in  the  filter  parameters  such  as  the  dimensional  stability 
derivatives.  The  analysis  is  expanded  to  explore  the  sensitivity  of  a 
system  of  higher  dimension  created  by  the  augmentation  of  an  additional 
state.  The  study  has  been  performed  by  independently  varying  each  of  the 
filter  parameters  over  a  given  range  and  noting  the  effect  on  the  accu- 
racy of  the  filter.  One  of  the  benefits  of  this  analysis  of  the  rms 
estimate  errors  to  variations  in  the  stability  derivatives  is  that  it 
reveals  which  derivatives  need  to  be  accurately  determined  to  ensure 
stable  flight. 
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I.   INTRODUCTION 

A  tactical  missile  normally  requires  midcourse  guidance  to  ensure 
that  its  trajectory  leads  to  a  specific  target.  A  typical  midcourse 
guidance  law  pre-programmed  strategy  maintains  constant  altitude,  heading 
and  speed.  Such  guidance  is  primarily  effected  by  an  Intertial  Navi- 
gation System  (INS).  In  this  work  two  steady-state  Kalman  Filters  (SKF), 
used  as  estimators  of  the  longitudinal  and  lateral  motion,  constitute 
what  may  be  considered  as  part  of  a  Strapdown  INS  onboard  a  missile  that 
can  be  cheaper  and  easier  to  implement  than  a  gimballed  INS.  The  authors 
of  [Ref.  1]  discuss  the  basic  differences  between  Strapdown  and  gimballed 
Inertial  Navigation  Systems. 

Sensors  on  the  missile  that  the  longitudinal  and  lateral  estimators 
could  use  are  described  by  Maybeck  [Ref.  2]  and  include  laser  rate  gyros, 
doppler  velocimeters,  magnetic  compasses,  and  barometric  altimeters.  A 
radar  seeker  could  provide  a  distance  or  range  measurement  or  range  rate. 
Distance  or  position  measurement  could  be  computed  from  a  signal  inserted 
into  the  missile's  INS  from  the  Global  Positioning  System  (GPS)  or 
similar  satellite-based  navigation  system. 

This  work  was  motivated  by  Bryson  [Ref.  3],  where  he  discusses  a 
Strapdown  INS  using  SKF  as  estimators  applied  to  the  model  for  the  DC-8 
airplane.  To  avoid  classification  requirements  and  for  convenience,  the 
model  used  here  is  essentially  the  same  as  that  of  [Ref.  3]  rather  than 
that  of  a  missile. 
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This  thesis  is  a  continuation  of  the  work  done  by  Matallana  [Ref.  4]. 
It  further  investigates  the  sensitivity  of  the  Kalman  Filter  to  inaccu- 
racies in  the  filter  parameters  or  varation  between  the  filter  model  and 
the  plant  model  for  longitudinal  motion  estimation.  The  differences 
could  be  due  to  model  inaccuracies  or  to  normal  variation  caused  by  a 
changing  flight  environment.  The  sensitivity  of  rms  estimate  errors  to 
inaccuracies  or  differences  in  the  stability  derivatives  is  the  result  of 
interest. 

The  initial  work  conducted  was  to  reproduce  the  results  of  [Ref.  3] 
and  [Ref.  4]  with  the  correct  implementation  of  the  dynamics  in  the 
filter  parameters.  Then  the  results  of  [Ref.  4]  for  the  longitudinal 
motion  estimator  with  incorrect  implementation  of  the  dynamics  in  the 
Kalman  Filter  were  reproduced. 

After  a  distance  measurement  and  associated  system  and  measurement 
noise  parameters  were  added  to  the  model  dynamics,  the  sensitivity  anal- 
ysis was  repeated  for  the  longitudinal  motion  estimator.  The  analysis  of 
the  effect  that  this  distance  input  had  on  the  sensitivity  of  the  rms 
errors  to  inaccuracies  or  differences  in  the  stability  derivatives  of  the 
Kalman  Filter  concluded  the  research  for  this  thesis. 
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II.  MODELS  AND  ESTIMATION 

A.   KALMAN  FILTER 

Only  a  brief  description  of  the  Kalman  Filter  has  been  included  to 
show  the  particular  formulation  used.  A  more  complete  development  of 
general  theory  is  done  by  Gelb  in  [Ref.  5]. 

1.  Linear  Dynamic  System 

Consider  the  linear  time  invariant  system  (plant  and  measurement 
models)  given  by  equation  (1)  below,  where  x  represents  the  states  of  the 
system;  z  is  the  measurement;  F  is  the  system  matrix;  T  is  the  driving 
noise  coefficient  matrix;  H  is  the  measurement  scaling  matrix;  and  w  and 
v  are  independent,  zero-mean,  white  gaussian  noise  processes  with  covar- 
iance  matrices  Q  and  R  respectively. 

x  =  Fx  +  Tw  (1-a) 

z  =  Hx  +  v  (1-b) 

Mathematically,  Q  and  R  are  represented  by  equation  (2)  as: 

E(w(t)wT(x))  =  Q(t)a(t-i),  E(w(t))  =  0  (2-a) 

E(v(t)vT(x))  =  R(t)a(t-x),  E(v(t))  =  0  (2-b) 

2.  Continuous  Kalman  Filter 

A  continuous  time  Kalman  Filter  is  described  by  equation  (3) 
where  x  is  the  state  estimate  and  K  is  a  matrix  of  constant  filter  gains. 

x  =  Fx  +  K(z-Hx)  (3) 
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The  implementation  of  the  System  Model  and  the  Kalman  Filter  is  shown  in 
Figure  1. 


MATHEMATICAL  MODEL 
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MEASUREMENT 
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Figure  1.  System  Model  and  Kalman  Filter 


The  estimate  error  is  defined  by  equation  (4)  as 


x  -  x  -  x 


(4) 


and  the  differential  equation  for  x  is  given  by 


x  =  (F-KH)x  -  Tw  +  Kv 


(5) 


The  differential  equations  for  the  states  of  a  linear  system  driven  by 
noise  can  be  expressed  as 


13 


X 
.X. 

= 

F-KH  0 

X 

_x_ 

+ 

Kv-rw 

L    o    FJ 

(6) 


The  covariance  of  the  estimate- error,  symbolized  as  P,  is  defined  by 
equation  (7).   It  provides  a  statistical  measure  of  the  uncertainty  in  x. 


P  =  E(2xT) 


(7) 


The  diagonal  elements  of  the  covariance  matrix  are  the  root  mean  square 
errors  of  the  state  variables.  Also,  the  trace  of  P  is  the  mean  square 
length  of  the  vector  x.  The  off  diagonal  terms  of  P  indicate  the  degree 
of  cross-correlation  between  the  elements  of  x.  The  covariance  matrix  P 
is  obtained  by  solving  the  linear  Lyapunov  equation  given  by 


P  =  (F-KH)  P  +  P(F-KH)T  +  TQrT  +  KRKT 


The  eigenvalues  of  the  filter  are  given  by  the  roots  of 


ISI  -  F  +  KHI  =  0 


(8) 


(9) 


B.   STATE  AUGMENTATION  AND  SHAPING  FILTERS 

When  the  system  random  disturbances  are  correlated  in  time,  i.e., 
colored  noise,  it  is  necessary  to  use  their  power  spectral  density  data 
in  order  to  develop  a  mathematical  model  that  produces  an  output  which 
duplicates  the  noise  characteristics  [Ref.  2].  Correlated  random  noises 
are  taken  to  be  state  variables  of  a  ficticious  linear  time  invariant 
system  (usually  called  a  shaping  filter)  which  is  itself  excited  by  white 
gaussian  noise.   Such  a  model  is  given  by  equation  (10)  below,  where  the 
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subscript  f  denotes  filter,  and  n  is  a  nonwhite  (time-correlated)  gaus- 
sian  noise.  The  filter  output  is  used  to  drive  the  system  depicted  by 
Figure  2. 


xf  =  ^fxf  +  ^~fw 


(10-a) 


zn  =  Hfxf 


(10-b) 


The  dimension  of  the  state  vector  (1)  is  increased  by  including  the 
disturbances  as  well  as  a  description  of  the  system  dynamics  behavior  in 
appropriate  rows  of  an  enlarged  F  matrix.  This  enlargement  process  is 
called  state  vector  augmentation. 
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Figure  2.   Shaping  Filter  Generating  Driving  Noise 


The  augmented  state  equation  is  given  by 
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LxfJ 
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rHfl 
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(11) 
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The  associated  measurement  equation  is 


z  = 


+  v 


(12) 


C.   SENSITIVITY  TO  PARAMETER  VARIATION 

Observing  the  structure  of  the  Kalman  Filter  illustrated  in  Figure  1, 
the  filter  contains  an  exact  model  of  the  system  dynamics. 

The  analysis  of  how  the  error  covariance  behaves  when  the  gain  matrix 
is  computed  using  perturbed  values  of  the  F  matrix,  such  as  varying 
parameters  due  to  different  flight  conditions,  is  well  explained  in 
[Ref.  5].  Figure  3  is  a  block  diagram  of  the  system  model  and  Kalman 
Filter  with  the  system  dynamics  perturbed.  F*  is  the  perturbed  system 
dynamics,  while  K*  is  the  associated  gain  matrix  computed  for  the  Kalman 
Filter. 
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Figure  3.   System  Model  and  Kalman  Filter  with 
Perturbed  Dynamics 
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The  equation  for  the  estimate  is  given  by 


x  =  F*x  +  K*(z-Hx) 


(13) 


The  error  in  the  estimate  is  given  by 


x  =  (F*  -  K*H)x  +  AFx  -  Tw  +  K*v 


(14) 


where 


AF  £  F*  -  F 


(15) 


The  differential  equations  for  the  states  of  linear  system  driven  by 
white  gaussian  noise  now  become 


F*  -  K*H 


AF 

X 

+ 

K*v-rw 

F 

X 

rw 

(16) 


Letting  x'  be  the  augmented  state  vector,  x1  - 
The  covariance  matrix  of  x1  is  given  by 


x 

X 


E(x'x' ')  = 


(17) 


where  one  defines  P  -  E(xx  ),  V  -  E(xx  ),  and  U  -  E(xx  ).  P,  the  covar- 
iance of  x,  is  the  quantity  of  interest.  The  error  sensitivity  equations 
are: 


P  =  (F*  -  K*H)  P  +  P(F*  -  K*H)T  +  AFV  +  VTAF  +  TQrT  +  K*RK*T 


(18-a) 


V  =  FV  +  V(F*  -  K*H)T  +  UAFT  -  TQrT 


(18-b) 
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and  T     T 

U  =  FU  +  UF1  +  TQr1  08-c) 


with  initial  conditions  P(0)  =  -V(0)  =  U(0)  =  E(x(0)x(0)T).  When  the 
actual  system  dynamics  are  reproduced  in  the  filter,  F  =  F*  and  AF  =  0, 
and  equation  (18)  reduces  to  the  linear  Lyapunov  equation  of  equation 
(8). 

D.   MODAL  COORDINATES  TRANSFORMATION 

The  system  represented  by  equation  (1)  is  not  unique.  Consider  an 
alternate  linear  transformaton  of  the  states  described  in  references  [3] 
and  [6].  Let  x  =  T,  where  |  represents  the  transformation  of  the  states 
and  T  is  the  transformation  matrix  with  the  columns  formed  by  the  eigen- 
vectors of  the  system  matrix  F  (for  a  complex  eigenvalue,  the  first 
column  is  the  real  part  and  the  second  is  the  imaginary  part  of  the 
eigenvector).   The  similarity  transformation  of  equation  (1)  is 

4  =  A4  +  Bw  (19-a) 

z  =  C4  +  v  (19-b) 

where  A  =  T"1  FT,  B  =  T~V,  and  C  =  HT. 

A  case  of  particular  interest,  the  canonical  form,  results  when  the  A 
matrix  is  diagonal  (i.e.,  when  the  eigenvalues  of  the  F  matrix  appear  on 
the  diagonal).  This  canonical  form  is  more  informative  than  the  transfer 
function  method,  since  observability  and  control  ability  of  the  system  can 
be  obtained  by  inspection. 
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E.   SOLUTION  OF  THE  SKF  WITH  A  PRESCRIBED  DEGREE  OF  STABILITY 

The  constant  gain  Kalman  Filter  (SKF)  used  as  an  observor  will 
diverge  if  undisturbed,  neutrally  stable  (UNS)  modes  are  in  the  system 
model.  In  references  [3]  and  [7]  the  authors  discussed  the  destabili- 
zation  of  the  system  model  (1).  The  amount  of  destabilization  can  be 
varied  until  the  suboptimal  observor  formed  has  a  desired  degree  of 
stability.  The  method  of  [Ref.  3]  destabilizes  only  the  UNS  modes  in  the 
system  model  and  is  called  "modal  destabilization"  (MDS).  In  this  tech- 
nique the  gains  of  the  filter  are  constrained  so  that 

Re(S.)  >  -a,  i  =  1,2, ,n  (20) 

where  Re(S.)  indicates  the  "real"  part  of  (S.),  S,,...,S  are  the  eigen- 
values of  the  filter,  i.e.,  the  roots  of  equation  (9),  and  a  is  a  speci- 
fied positive  number. 

The  original  system  model  is  destabilized  in  accordance  with  equation 
(21),  where  F'  is  the  destabilized  matrix  formed,  E  is  the  destabili- 
zation matrix  (diagonal),  and  T  is  the  modal  transformation  matrix 
(eigenvector  matrix).  The  matrix  F'  is  used  to  calculate  the  suboptimal 
gains  of  the  filter. 

F1  =  F  +  TET"1  (21) 

This  MDS  approach  prevents  the  divergence  of  the  steady-state  Kalman 
Filter  in  a  system  with  UNS  model  while  causing  only  a  slight  reduction 
in  the  estimation  accuracy. 
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III.  DYNAMIC  AND  MEASUREMENT  SYSTEM  MODELS 

A.   REFERENCE  AXIS  SYSTEM 

The  Reference  Axis  System  of  a  missile  is  centered  at  its  center  of 
gravity  (e.g.)  and  fixed  on  the  missile  body  as  follows: 

X  axis,  the  roll  axis,  forward  from  the  e.g.  along  the  axis  of 
symmetry. 

Y  axis,  the  pitch  axis,  outward  to  the  right  from  the  e.g.  when 
viewing  the  missile  from  behind. 

Z  axis,  the  yaw  axis,  downward  from  the  e.g.  in  the  plane  of  sym- 
metry to  form  a  right-handed  orthogonal  system  with  the 
other  two. 

Appendix  A  lists  the  symbols  defining  quantities  associated  with  the 
missile  illustrated  in  Figure  4  below  such  as  forces  and  moments,  linear 
and  angular  velocities,  and  moments  of  inertia. 


X,V,u 


Relative  Wind 


Figure  4.  Reference  Axis  System 
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B.  MISSILE  EQUATIONS  OF  MOTION 

The  equations  of  motion  used  to  represent  the  missile  dynamics  used 
in  this  study  are  well  defined  in  [Ref.  8].  A  linear  dynamical  model  of 
the  missile  based  on  the  rigid  body  approximation  is  appropriate. 

1 .   Longitudinal  Motion 

The  longitudinal  motions  of  a  missile  can  be  modeled  by  a  fifth- 
order  system  of  equation  (22),  where  the  state  variables  are  u,  velocity 
along  the  X  axis,  w,  velocity  along  the  Z  axis,  q,  pitch  rate,  9,  pitch 
angle  and  h,  altitude.  The  units  are:  u  and  w  in  10  ft/s,  q  in  0.01 
rad/s,  9  in  0.01  rad,  and  h  in  100  ft. 


u 
w 

q 

9 
h 


Xu  Xw  0     -g 

Zu  Zw  V      0 

Mu+MwZu  Mw+MwZw  Mq+MwV     0 

0  0  10 

0  -0.1  0      V 


o" 

u™ 

0 

w 

0 

q 

0 

9 

0. 

h 

(22) 


2.   Lateral  Motion 

The  lateral  motions  of  a  missile  are  modeled  by  the  fifth-order 
system  given  by  equation  (23),  where  the  state  variables  are:  p,  sideslip 
angle,  r,  yaw  rate,  p,  roll  rate,  <J>,  roll  angle,  and  t|»,  heading  angle. 
The  units  are:  B  in  rad,  r  in  rad/s,  p  in  rad/s,  9  in  rad,  and  tj>  in  rad. 
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p 

Yv 

-1 

0 

g/v 

0 

P 

r 

Np 

N1 
r 

NP 

0 

0 

p 

P 

= 

lp 

Lr 

LP 

0 

0 

p 

i 

0 

0 

1 

0 

0 

0 

i 

0 

1 

0 

0 

0 

>j> 
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C.  MODEL  DYNAMICS 

The  aerodynamic  data  used  in  this  paper  appears  in  Appendix  B. 
Except  for  the  addition  of  system  and  measurement  noise  parameters  for 
the  distance  input,  the  models  and  noise  dynamics  are  the  same  as  those 
of  [Ref.  3]. 

1.  Longitudinal  Motion  Estimation 

The  main  disturbance  inputs  are  the  two  wind  velocities  u  and  w  . 

g    g 

Under  certain  flight  conditions,  the  turbulance  represented  by  the  fluc- 
tuating parts  of  u  and  w  are  colored  noise.  They  are  modeled  by  first- 
order  shaping  filters  with  white  gaussian  noise  inputs  as  shown  in 
equation  (10).  The  linear  model  that  results  is  given  by  equation  (24) 
[Ref.  4]. 


-0.413 


0.413 


(24) 


w 


-0.853 


w 


0.853 
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The  numerical  data  for  the  longitudinal  dimensional  derivatives 
was  used  in  equation  (22).  The  resultant  model  is  represented  by  equa- 
tion (25)  which  corresponds  to  the  state  vector  augmentation  of  equation 

(11).   Scaling  is  done  with  u,  w,  u  ,  and  w  in  units  of  10  ft/s,  q  in 

y      y 

units  of  0.01  rad/s,  0  in  units  of  0.01  rad,  and  h  in  units  of  100  ft. 


u 
w 

* 

q 

9 
h 

w 


0.015 

0.004 

0 

-0.0322 

0 

-0.015 

0.074 

-0.806 

0. 

824 

0 

0 

-0.074 

0.749 

-10.7 

-1. 

344 

0 

0 

-0.749 

0 

0 

1 

0 

0 

0 

0 

-0.1 

0 

0.0824 

0 

0 

0 

0 

0 

0 

0 

-0.413 

0 

0 

0 

0 

0 

0 

0.004 

u 

-0.806 

w 

-10.7 

q 

0 

9 

0 

h 

0 

ug 

-0.853 

.\ 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0.413 

0 

0 

0.853 

w 


(25) 


The  measurement  model  shown  by  equation  (26)  assumes  a  rate  gyro 
in  order  to  measure  z  and  a  barometric  altimeter  to  measure  z.  . 
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LrhJ 


0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

1 

0 

u 

w 

q 

0 

h 

ug 

w 

lV 


(26) 


2.   Lateral  Motion  Estimation 

The  main  disturbance  input  is  the  lateral  wind  v.  The  turbulence 
represented  by  the  fluctuating  part  of  v  is  the  colored  noise,  which  is 
also  modeled  as  a  first-order  shaping  filter  with  white  gaussian  noise 
input  as  given  by  equation  (10).  The  resulting  shaping  filter  taken  from 
[Ref.  3]  is  given  by  equation  (27). 


B  =  -0.853B  +  0.853u 


(27) 


where  B  =  v  /V. 
Hg   9 


The  numerical  data  for  the  lateral  dimensional  derivatives  was 
applied  in  equation  (23)  to  obtain  equation  (28),  which  corresponds  to 
the  state  vector  augmentation  of  equation  (11). 


p 

r 

P 

i 

i 

K 

0.0868 

-1 

0 

0.03907  0 

2.14 

-0. 

228 

-0. 

0204 

0       0 

4.41 

0. 

334 

-1. 

181 

0       0 

0 

0 

1 

0       0 

0 

1 

0 

0       0 

0 

0 

0 

0       0 

-0.0868 

P 

0 

2.14 

r 

0 

-4.41 
0 

P 

0 

+ 

0 
0 

0 

«1> 

0 

-0.853 

.\ 

0.853 

_    _ 

(28) 
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The  measurement  model  given  by  equation  (29)  below  represents  the  case 
where  the  measurement  z  is  taken  with  a  roll-rate  gyro  and  the  measure- 
ment z  obtained  from  a  magnetic  compass. 


■* 


0    0    10    0    0 
0     0    0    0    10 


P 

r 

1        r     ~i 

P 

+ 

1 

0 

V 

P 

<i> 

0 

1 

\ 

<J> 

-J               L        _ 

s 

(29) 
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IV.      ANALYSIS 

A.  SIMULATION 

The  Sensitivity  Covariance  Program  developed  for  application  in  the 
work  of  [Ref.  4]  was  used  to  solve  the  error  sensitivity  equations  of 
equation  (18).  The  program  was  originally  developed  to  handle  a  set  of 
105  linear  differential  equations  for  the  longitudinal  case  and  78  for 
the  lateral.  The  program  was  revised  to  accommodate  136  linear  differ- 
ential equations  in  the  longitudinal  case  and  101  in  the  lateral,  to 
allow  for  an  additional  state  augmentation  (i.e.,  the  distance  measure- 
ment to  the  longitudinal  model).  The  outputs  of  these  programs  are  the 
time  matrices  and  rms  estimate  errors,  the  square  roots  of  the  diagonal 
elements  of  the  P  matrices.  The  OPTSYS  program,  the  use  of  which  is 
described  by  [Ref.  9]  and  amplified  by  [Ref.  10],  was  applied  to  calcu- 
late the  Kalman  Filter  gains  to  be  inserted  into  the  Sensitivity  Covar- 
iance Program  to  find  the  estimate  errors  for  specific  system  parameter 
perturbations.  The  OPTSYS  program  was  also  used  to  destabilize  the 
systems  that  contained  UNS  modes  in  an  attempt  to  eliminate  filter 
divergence.  Copies  of  the  OPTSYS  and  the  Sensitivity  Covariance  programs 
follow  under  COMPUTER  PROGRAMS,  while  a  copy  of  [Ref.  10]  appears  in 
Appendix  C. 

B.  RESULTS 

As  the  problem  is  introduced,  the  results  are  presented  in  three 
parts:  (1)  to  verify  the  findings  of  [Ref.  3]  and  [Ref.  4]  with  the 
correct  implementation  of  the  dynamics  in  the  filter  parameters,  (2)  to 
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reproduce  the  findings  of  [Ref.  4]  for  the  longitudinal  motion  estimator 
with  incorrect  implementation  of  the  dynamics  in  the  Kalman  Filter,  and 
(3)  to  conduct  a  sensitivity  analysis  of  the  longitudinal  motion  esti- 
mator after  adding  a  distance  measurement  and  associated  system  and 
measurement  noise  parameters  to  the  model  dynamics. 
1 .  Motion  Estimation  Analysis  for  Exact  Dynamics 

The  OPTSYS  program  was  used  with  input  data  representing  the 
actual  system  dynamics  for  both  the  longitudinal  and  lateral  cases  to 
obtain  the  following  results  which  are  the  same  as  those  of  [Ref.  4]  and 
essentially  the  same  as  those  of  [Ref.  3]. 

a.  Longitudinal  Case 


filter 

gain 

matrix  K 

filter  eigenvalues 

0.059 

0.060 

-0.310  +  J0.411 

0.264 

-0.161 

-0.429 

3.517 

0.040 

-0.178 

0.001 

-0.080 

-0.261 

-0.011 

0.035 

-0.063  +  jO.0743 

-1.288 

0.128 

rms  estimate  errors 

u  =  2.090  ft/s         0  =  0.317  deg 

w  =  5.102  ft/s         h  =  8.245  ft 

q  =  0.416  deg/s        u  =  4.776  ft/s 

w  =5.701  ft/s 
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b. 


Lateral 

Case 

i 

filter 

gain 

matrix  K 

0.051 

-0.967 

-1.536 

0.411 

2.695 

-0.004 

0.386 

-0.789 

-0.005 

0.906 

-1.713 

0.655 

filter  eigenvalues 

-2.350  +  j'2.594 
-0.624  +  jO.492 
-0.00125 
0.0 


rms  estimate  errors 

vQ  =  3.329  ft/s 
P 

f  =  0.244  deg/s 

p  =  0.377  deg/s 

<j>  =  0.222  deg 

i  -   0.214  deg 

VQ  =  5.506  ft/s 

2.   Longitudinal  Motion  Estimation  Analysis 

The  0PTSYS  program  was  used  to  compute  a  new  K*  matrix  as  each 
parameter  of  the  F  matrix  was  individually  numerically  varied.  The 
Sensitivity  Covariance  Program  was  then  executed  utilizing  each  new  K* 
and  F*  matrix  pair  to  determine  the  rms  errors  for  each  individual  per- 
turbation. 

The  results  are  shown  in  Tables  1-8  and  are  identical  to  those  of 
[Ref.  4].  The  true  values  for  the  unperturbed  system  dynamics  parameters 
are  indicated  in  the  tables  by  an  asterisk.  A  discussion  of  the  results 
fol lows: 
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X  .  The  dimensional  variation  of  the  X  force  with  forward  speed  u 

has  a  nominal  value  of  -0.015.   This  quantity  was  varied  in  a 

range  of  ±20%.   The  behavior  of  the  rms  estimate  errors  can  be 

seen  in  Table  1.   The  tabulation  shows  that  the  numerical 

variation  of  the  X   derivative  does  not  cause  significant 

u  3 

changes  in  the  nominal  values  of  the  rms  estimate  errors  of  the 
states  w,  q,  8,  u  ,  and  w  .  The  states  u  and  h  appear  to  be 
slightly  effected,  but  not  enough  to  be  of  importance. 


u 


w 


X  .  The  dimensional  variation  of  the  X  force  with  downward  speed  w 
w 

has  a  nominal  value  of  0.004.  Again,  a  numerical  variation  in 

a  range  of  ±20%  was  conducted.  The  behavior  of  the  rms  errors 

is  demonstrated  by  Table  2.   Comparing  these  values  with  the 

nominal  ones  reveals  that  changes  in  the  X  derivative  have 

_  _  _  w_ 

essentially  no  effect  on  the  states  w,  q,  9,  u  ,  and  w  ,  while 
the  states  u  and  h  show  changes  too  small  to  consider  important. 


Z„.  The  dimensional  variation  of  the  Z  force  caused  by  a  change  in 
the  forward  speed  u  has  a  nominal  value  of  -0.074.  The  design 
value  was  altered  in  a  range  of  ±20%  with  the  results  shown  in 
Table  3.  Evaluation  of  this  data  indicates  that  all  the  rms 
errors  show  some  sensitivity  except  for  that  of  q.  The  most 
significant  changes  occur  in  the  u,  9,  and  h  states.  The  large 
variation  in  u  can  be  important  in  terms  of  the  accuracy  in 
radial  position. 


Z,,.  The  dimensional  variation  of  the  Z  force  with  downward  speed  w 
has  a  nominal  value  of  -0.806.  The  results  for  this  case  with 
changes  in  Z  over  a  range  of  ±20%  follow  in  Table  4.  They 
show  that  all  the  rms  estimate  errors  are  quite  sensitive  and 
any  variation  of  Z  beyond  ±2%  can  be  considered  critical  and 
unacceptable. 
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TABLE  1.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Xy  DERIVATIVE 


Xu 

u 
ft/s 

w 
ft/s 

q 

deg/s 

8 

deg 

h 
ft 

~ug 

ft/s 

w 

g 

ft/s 

-0.018 

2.096 

5.102 

0.416 

0.317 

8.248 

4.776 

5.701 

-0.0165 

2.094 

5.102 

0.416 

0.317 

8.246 

4.776 

5.701 

-0.01575 

2.091 

5.102 

0.416 

0.317 

8.240 

4.776 

5.701 

-0.015  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.01425 

2.088 

5.103 

0.416 

0.317 

8.260 

4.775 

5.701 

-0.0135 

2.089 

5.103 

0.416 

0.317 

8.280 

4.775 

5.701 

-0.012 

2.092 

5.103 

0.416 

0.317 

8.340 

4.775 

5.701 

TABLE  2.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 

WITH  VARIATION  IN  X  DERIVATIVE 

w 


Xw 

u 
ft/s 

w 
ft/s 

q 

deg/s 

8 
deg 

h 
ft 

u 

g 

ft/s 

w 

g 

ft/s 

0.0048 

2.070 

5.102 

0.416 

0.317 

8.319 

4.776 

5.701 

0.0044 

2.080 

5.102 

0.416 

0.317 

8.282 

4.776 

5.701 

0.0042 

2.086 

5.102 

0.416 

0.317 

8.257 

4.776 

5.701 

0.004  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

0.0038 

2.100 

5.102 

0.416 

0.317 

8.223 

4.776 

5.701 

0.0036 

2.103 

5.102 

0.416 

0.316 

8.215 

4.776 

5.701 

0.0032 

2.110 

5.101 

0.416 

0.316 

8.180 

4.776 

5.701 
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TABLE  3.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  Z  DERIVATIVE 


Zu 

u 
ft/s 

w 
ft/s 

q 

deg/s 

8 

deg 

h 
ft 

"ug 

ft/s 

w 

g 

ft/s 

-0.0888 

1.885 

5.106 

0.416 

0.322 

9.310 

4.776 

5.707 

-0.0814 

1.974 

5.104 

0.416 

0.319 

8.808 

4.775 

5.703 

-0.0777 

2.026 

5.194 

0.416 

0.318 

8.579 

4.775 

5.702 

-0.740  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0703 

2.122 

5.100 

0.416 

0.315 

7.800 

4.777 

5.700 

-0.0666 

2.270 

5.095 

0.416 

0.313 

7.101 

4.777 

5.697 

-0.0592 

2.32 

5.094 

0.416 

0.311 

7.000 

4.778 

5.695 

TABLE  4.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 

WITH  VARIATION  IN  Z  DERIVATIVE 

w 


Zw 

u 
ft/s 

w 
ft/s 

q 

deg/s 

0 
deg 

h 

ft 

u 

g 

ft/s 

w 

g 

ft/s 

-0.9612 

30.08 

6.172 

0.486 

0.440 

17.97 

4.778 

7.138 

-0.8866 

11.80 

5.810 

0.428 

0.415 

33.50 

4.785 

5.957 

-0.8463 

5.345 

5.259 

0.421 

0.400 

22.776 

4.779 

5.737 

-0.806  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.7657 

2.668 

5.032 

0.412 

0.219 

16.903 

4.772 

5.710 

-0.7256 

3.188 

5.035 

0.407 

0.200 

21.026 

4.760 

5.746 

-0.665 

3.260 

5.065 

0.406 

0.185 

23.000 

4.767 

5.794 
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M  .  The  dimensional  variation  of  the  M  moment  caused  by  a  change  in 
the  forward  speed  u  has  a  nominal  value  of  -0.000786.  From 
Table  5,  one  notes  that  the  rms  errors  for  the  states  q,  u  , 
and  w  are  not  effected  by  a  variation  in  M  of  ±20%,  but 
significant  changes  are  seen  when  M  is  varied  more  than  ±10% 
in  the  errors  of  states  u,  w,  6,  and  h. 

M  .  The  dimensional  variation  of  the  M  moment  with  speed  w  has  a 
nominal  value  of  -0.0111.  The  results  of  a  numerical  variation 
in  a  range  of  ±10%  can  be  seen  in  Table  6.  Since  any  alter- 
ation in  the  true  value  of  M  has  a  strong  effect  on  all  the 
rms  estimate  errors,  this  derivative  can  be  considered  the  most 
critical  in  the  longitudinal  motion  estimation  case. 


M  .  The  dimensional  variation  of  the  pitching  moment  with  pitch 

rate  q  has  a  nominal  value  of  -0.924.   The  results  of  Table  7 

on  the  rms  estimate  errors  for  a  ±20%  change  in  M  show  that 

q 

the  sensitivity  to  variations  in  this  parameter  is  minimal  for 
all  states. 


M*.  The  dimensional  variation  of  the  pitching  moment  with  the  rate 

of  change  of  the  downward  speed  w  has  a  nominal  value  of 

-0.00051.   Table  8  contains  the  rms  errors  data  obtained  by 

altering  M*  ±20%  from  its  nominal  value.  All  the  errors  show  a 
3  w 

degree  of  sensitivity  and  the  variation  of  errors  is  signifi- 
cant when  M*  is  changed  by  more  than  ±2%. 


Since  plots  of  the  rms  estimate  errors  versus  changes  in  the 
particular  dimensional  derivatives  for  data  identical  to  that  of  Tables 
1-8  appears  in  [Ref.  4]  in  Figures  35-40,  they  will  not  be  repeated  in 
this  work.  A  summary  of  the  relative  sensitivity  of  the  rms  estimate 
errors  to  changes  in  the  individual  dimensional  derivatives  follows  in 
Table  9. 
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TABLE  5.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  M  DERIVATIVE 


Mu 

u 
ft/s 

w 
ft/s 

q 

deg/s 

8 

deg 

h 
ft 

~ug 

ft/s 

w 

g 

ft/s 

-0.000943 

2.234 

5.061 

0.416 

0.305 

6.230 

4.775 

5.689 

-0.000865 

2.115 

5.089 

0.416 

0.310 

6.640 

4.775 

5.695 

-0.000825 

2.104 

5.094 

0.416 

0.314 

7.531 

4.776 

5.698 

-0.000786* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.000747 

1.993 

5.105 

0.416 

0.318 

8.595 

4.777 

5.703 

-0.000707 

1.866 

5.108 

0.416 

0.319 

8.832 

4.779 

5.705 

-0.000629 

1.566 

5.111 

0.416 

0.322 

9.178 

4.783 

5.708 

TABLE  6.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 

^  WITH  VARIATION  IN  M  DERIVATIVE 

w 


M 
w 

u 
ft/s 

w 
ft/s 

q 
deg/s 

8 

deg 

h 
ft 

~ug 

ft/s 

w 

g 

ft/s 

-0.01165 

18.43 

8.350 

0.502 

0.455 

29.870 

4.778 

5.695 

-0.0113 

5.163 

5.142 

0.433 

0.373 

17.790 

4.778 

5.694 

-0.0112 

3.110 

5.113 

0.418 

0.321 

10.427 

4.777 

5.699 

-0.0111    * 

2.090 

51.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.0109 

5.206 

5.018 

0.419 

0.325 

16.650 

4.776 

5.700 

-0.01055 

13.652 

5.342 

0.447 

0.430 

12.204 

4.776 

5.918 

-0.00999 

19.13 

18.95 

0.475 

0.704 

68.98 

4.756 

6.102 
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TABLE  7.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 
WITH  VARIATION  IN  M  DERIVATIVE 

q 


Mq 

u 
ft/s 

w 
ft/s 

q 

deg/s 

8 

deg 

h 

ft 

"ug 

ft/s 

w 

g 

ft/s 

-1.109 

2.091 

5.102 

0.416 

0.316 

8.230 

4.776 

5.701 

-1.016 

2.091 

5.102 

0.416 

0.316 

8.234 

4.776 

5.701 

-0.970 

2.091 

5.102 

0.416 

0.317 

8.236 

4.776 

5.701 

-0.924  * 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.880 

2.090 

5.102 

0.416 

0.316 

8.244 

4.776 

5.701 

-0.832 

2.089 

5.102 

0.416 

0.316 

8.236 

4.776 

5.701 

-0.7392 

2.089 

5.102 

0.416 

0.316 

8.232 

4.776 

5.701 

TABLE  8.   RMS  ESTIMATE  ERRORS  FOR  LONGITUDINAL  MOTION  ESTIMATOR 

WITH  VARIATION  IN  M-  DERIVATIVE 

w 


w 

u 
ft/s 

w 
ft/s 

q 

deg/s 

0 
deg 

h 

ft 

u 

g 

ft/s 

5g 

ft/s 

-0.00061 

2.610 

5.053 

0.417 

0.273 

10.665 

4.775 

5.688 

-0.00056 

2.476 

5.089 

0.417 

0.295 

6.301 

4.776 

5.703 

-0.00053 

2.398 

5.091 

0.417 

0.304 

4.150 

4.776 

5.698 

-0.00051* 

2.090 

5.102 

0.416 

0.317 

8.245 

4.776 

5.701 

-0.00049 

2.491 

5.108 

0.416 

0.322 

9.757 

4.776 

5.702 

-0.00046 

2.976 

5.118 

0.415 

0.332 

12.067 

4.776 

5.703 

-0.00041 

3.570 

5.124 

0.415 

0.340 

13.536 

4.776 

5.703 
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TABLE  9.   RELATIVE  SENSITIVITY  OF  THE  RMS  ESTIMATE  ERRORS 
TO  CHANGES  IN  DERIVATIVES 


DERIVATIVE 

NS 

RS 

VS 

X 
u 

X 

X 
w 

X 

Zu 

X 

Zw 

X 

Mu 

X 

Mw 

X 

Mq 

X 

M; 

X 

NS  =  Not  sensitive 

RS  =  Relatively  sensitive 

VS  =  Very  sensitive 
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3.  Analysis  of  Longitudinal  Motion  Estimation  After  Augmentation 

Including  the  distance  measurement  to  the  system  required  state 
augmentation  of  the  system  and  measurement  models  as  demonstrated  by 
equations  (30)  and  (31)  that  follow: 


u 

• 

w 

• 

q 

8 

= 

h 

. 

ug 

• 

wg 

d 

—  — 

0.015 

0.004 

0 

-0. 

0322 

0 

-0.015 

0.074 

-  0.806 

0.824 

0 

0 

-0.074 

0.749 

-10.7 

-1.344 

0 

0 

-0.749 

0 

0 

1.0 

0 

0 

0 

0 

-  0.1 

0 

0. 

824 

0 

0 

0 

0 

0 

0 

0 

-0.413 

0 

0 

0 

0 

0 

0 

1.0 

0 

0 

0 

0 

0 

0.004 

0 

u 

-  0.806 

0 

w 

-10.7 

0 

q 

0 

0 

e 

0 

0 

h 

0 

0 

ug 

-0.853 

0 

\ 

0 

0_ 

_d_ 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0.413 

0 

0 

0 

0. 

853 

0 

0 

0 

1.0 

u„1 


w 


MwJ 


(30) 
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and 


0  0  10  0  0  0  0 
0  0  0  0  10  0  0 
0   0   0   0   0   0   0   1 


0 

0 

V 

q 

1 

0 

vh 

0 

1 

>_ 

(31) 


In  these  equations  the  state  variables  are  u,  velocity  along  the  X  axis, 
w,  velocity  along  the  Z  axis,  q,  pitch  rate,  6,  pitch  angle,  h,  altitude 
and  d,  distance  traveled  along  the  X  axis.  The  units  are:  u  and  w  in 
10  ft/s,  q  in  0.01  rad/s,  6  in  0.01  rad,  h  in  100  ft,  and  d  in  10  ft. 
The  OPTSYS  program,  when  executed  with  the  data  from  equations 
(30)  and  (31)  above  and  the  standard  deviation  values  from  Appendix  B, 
yielded  the  following: 

filter  gain  matrix  K 
0.0592  0.0570  0.0014 
0.2646  -0.1611  -0.0007 
3.5168  0.0404  0.0002 
0.0011  -0.0810  -0.0007 
0.0135  0.1353  0.0001 
-0.0114  0.0356  -0.0002 
-1.2876  0.1283  0.0005 
0.0469     0.0561     1.0014 
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filter  eigenvalues 

-3.103  ±  4.1103 

-1.000 

-0.4146 

-0.3152 

-0.0485  ±  0.0548 

-0.0551 

rms  estimate  errors 

u  =  2.090  ft/s  9  =  0.316  deg 

w  =5.102  ft/s  h  =  8.225  ft 

q  =  0.416  deg/s  u  =  4.776  ft/s 

w  =  5.701  ft/s  d  =  38.730  ft 

The  next  step  in  the  analysis  was  to  perturb  each  directional 
derivative  independently  by  specific  amounts  from  its  nominal  value  and 
to  observe  the  effect  on  the  rms  estimate  errors.  This  process  was 
carried  out  for  all  eight  directional  derivatives  and  the  response  was 
the  same  for  each  case  --  even  a  slight  perturbation  of  -0.1%  of  any 
directional  derivative  from  its  nominal  value  caused  the  rms  estimate 
errors  to  increase  without  bound.  This  behavior  indicated  that  any 
incorrect  implementation  of  dynamics  in  the  new  system  formed  by  the 
augmentation  of  the  distance  measurement  would  cause  instability  and  the 
Kalman  Filter  to  diverge. 

Several  system  parameters  were  individually  modified  and  the 
analysis  repeated  in  hopes  of  finding  a  stable  system  for  which  the 
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Kalman  Filter  converged.  The  coefficient  for  the  distance  term  in  the 
process  noise  distribution  matrix  was  varied  from  0.01-5.0,  the  power 
spectral  density  process  noise  entry  for  distance  was  changed  in  a  range 
of  1.105-30.0,  and  the  distance  term  for  the  power  spectral  density 
measurement  noise  adjusted  over  a  range  of  0.03-30.0.  None  of  these 
trials  led  to  a  stable  system. 

A  modal  analysis  was  also  performed  using  the  open  loop  eigen- 
values from  the  0PTSYS  output  listing.  The  system  of  equations  (30)  and 
(31)  when  transformed  into  modal  coordinates  give  equations  (32)  and  (33) 
below: 


V 

i* 

t.1 

trf 

= 

<P1 

^ 

H 

H 

0     0 

0 

0 

0 

0 

0 

0     0 

0 

0               0 

0 

0 

0    o    ■ 

■1.076 

2.957       0 

0 

0 

0     0 

2.957     ■ 

•1.076       0 

0 

0 

0     0 

0 

0             -0.006 

0.024 

0 

0     0 

0 

0               0.024 

-0.006 

0 

0     0 

0 

0               0 

0 

-0.413 

0     0 

0 

0               0 

0 

0 

"o 

0.1 

o" 

-1.0 

0 

1.0 

-0.028 

-0.088 

0 

-0.110 

-3.153 

0 

»u 

1.027 

-0.048 

0 

^w 

-0.210 

1.467 

0 

.Md. 

0.420 

0 

0 

0 

1.836 

0 

0 

V 

0 

^ 

0 

«,1 

0 

hz 

0 

£pl 

0 

%2 

0 

^ 

-0.853 

i\ 

(32) 
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0  0  1.0  0  -0.0001 
1.0  0  0.0016  0.0016  -0.0156 
0   1.0  -0.0008  -0.0004   1.0 


0.0005  0.0548   0.4802 

■0.0612     0.0082     -0.0025 

0  -0.0657     -0.0252 


1 

0 

0 

V 

g 

0 

1 

0 

vh 

0 

0 

1 

>. 

V 

*d 

6.1 

«s2 

^P' 

^p2 

*U9 

1\ 

(33) 


Consistent  with  the  discussion  in  [Ref.  3],  inspection  of  equa- 
tion (32)  revealed  that  the  energy  mode  £r  and  the  distance  mode  4  ,,  were 
neutrally  stable  (i.e.,  eigenvalues  =  0).   Inspection  of  equation  (33) 

showed  that  |c  was  unobservable  with  z  and  z  .  and  that  £ ,  was  unobserv- 
*t  q     d        ^d 

able  with  z  and  z..   Equation  (32)  also  disclosed  that  £r   was  undis- 
turbed by  u  and  d,  and  that  £  .  was  undisturbed  by  w  .   Therefore,  de- 
g  d  g 

stabilization  was  conducted  in  an  attempt  to  prevent  filter  divergence. 
Both  total  and  modal  destabil ization  described  earlier  in  this  work  and 
in  [Ref.  3]  were  performed  in  amounts  of  0.040  and  1.0  using  the  0PTSYS 
program.  The  filter  gains  computed  for  the  destabilized  system  were  then 
executed  in  the  Sensitivity  Covariance  Program  with  each  of  the  modified 
parameter  combinations  discussed  earlier.  Without  exception,  the  rms 
estimation  errors  increased  without  bound  when  the  least  sensitive  dimen- 
sional derivative  X  was  perturbed  by  as  little  as  ±1%. 
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V.   CONCLUSIONS  AND  RECOMMENDATIONS 

A.   CONCLUSIONS 

The  conclusions  reached  were  based  on  the  results  obtained  and  will 
therefore  be  presented  in  three  parts. 

1.  Motion  Estimation  Analysis  for  Exact  Dynamics 

The  data  in  the  results  is  in  agreement  with  that  of  both 
[Ref.  3]  and  [Ref.  4].  It  shows  that  both  the  Kalman  Filters  for  initial 
longitudinal  and  lateral  cases  are  stable  when  the  true  values  for  the 
system  dynamics  are  implemented. 

2.  Longitudinal  Motion  Estimation  Analysis 

The  results  from  Tables  1-8  and  summarized  in  Table  9  are  con- 
sistent with  those  of  [Ref.  4].   The  stability  derivatives  Z   and  M 

W  W 

cause  the  strongest  changes  in  the  rms  estimate  errors  when  they  are 
varied.   Basically,  Z  and  M  must  be  quite  accurately  reproduced  in  the 

W  W 

filter  to  prevent  divergence.   Changes  in  the  stability  derivatives  Z  , 

M.  and  M*  reflect  intermediate  variations  in  nearly  all  the  rms  estimate 
u      w  * 

errors.   For  the  model  considered  a  tolerance  of  more  than  ±5%  affects 

the  accuracy  in  the  radial  position  since  large  variations  in  u  occur.  A 

tollerance  of  perhaps  ±20%  can  be  accepted  in  the  dimensional  derivatives 

X  .  X  ,  and  M  for  this  model  since  no  important  effect  is  noted  in  the 
u   w      q 

rms  errors  over  that  range. 

3.  Analysis  of  Longitudinal  Motion  Estimation  After  Augmentation 
From  the  results  presented  earlier  for  the  new  system  model  fomed 

by  the  augmentation  of  a  distance  measurement  and  associated  process  and 
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measurement  noise  parameters,  it  is  apparent  that  the  corresponding 
Kalman  Filter  will  diverge  for  even  a  slight  variation  in  any  of  the 
dimensional  derivatives  from  their  nominal  values.  Even  the  Kalman 
Filter  developed  by  system  destabilization  proved  to  be  unstable  with  the 
parameters  used. 

B.   RECOMMENDATIONS 

Further  analysis  of  the  augmented  system  including  the  distance  or 
position  estimation  is  desirable.  Perhaps  a  more  in-depth  study  of  the 
measurement  parameter  scaling  would  enable  the  development  of  a  stable 
Kalman  Filter  for  at  least  a  destabilized  system. 
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VI.   SUMMARY 

The  sensitivity  analyses  performed  in  this  work  have  revealed  the 
importance  of  accuracy  in  determining  system  dynamics  utilized  in  formu- 
lating the  model  for  the  Kalman  Filter.  The  relative  sensitivity  of  the 
rms  estimation  errors  to  variance  in  each  of  the  particular  dimensional 
derivatives  is  shown  in  Table  9  for  the  Longitudinal  Motion  Estimator. 

The  longitudinal  system  augmented  with  the  distance  measurement 
developed  appears  to  be  extremely  sensitive  to  variations  in  all  the 
dimensional  derivatives.  Further  analysis  of  the  model  developed  is 
suggested. 
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APPENDIX  A 
LIST  OF  SYMBOLS 

Regular  Symbols  Definition 

A  Modal  transformation  of  F  matrix 

B  Modal  transformation  of  r  matrix 

C  Modal  transformation  of  H  matrix 

D  Dutch  roll  mode 

d  Distance  traveled  along  the  X  axis 

E  Destabilization  matrix 

F  System  dynamics  matrix 

f  Subscript  for  filter 

F1  Destabilized  matrix 

g  Subscript  for  wind  speed 

H  Measurement  matrix 

H  Heading  Mode 

h  Altitude 

INS  Inertial  Navigation  System 

K  Kalman  Filter  gain  matrix 

L  Rolling  moment  (about  X  axis) 

M  Pitching  moment  (about  Y  axis) 

MDS  Modal  destabilization 

N  Yawing  moment  (about  Z  axis) 

n  Non-white  gaussian  noise 

P  Covariance  propagation  of  the  estimate 

error  matrix 

P  Perturbed  roll  rate 

Q  Covariance  matrix  of  w 

q  Perturbed  pitch  rate 

R  Covariance  matrix  of  v 

r  Perturbed  yaw  rate 

S  Spiral  mode 
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Regular  Symbols  Definition 

SKF  Steady-State  Kalman  Filter 

T  Transformation  matrix 

UNS  Undisturbed  neutrally  stable 

u  Perturbed  forward  speed  (along  X  axis) 

V  Forward  velocity 

v  Perturbed  side  velocity 

w  Driving  white  gaussian  noise 

w„  Perturbed  downward  velocity 
9 

X  Reference  axis 

x  State  vector  of  the  system 

x  State  estimate  vector 

x  Estimate  error  vector 

Y  Reference  axis 

z  Measurement  vector 

Z  Reference  Axis 


Greek  Symbols  Definition 

<Ji  Heading  angle 

0  Perturbed  pitch  attitude  angle 

<}>  Perturbed  bank  (roll)  angle 

B  Sideslip  angle 

r  Driving  noise  matrix 

a  Eigenvalue  constrain 

a  Standard  deviation 

4  Transformed  state  vector 

t  Time 
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APPENDIX  B 
AERODYNAMIC  DATA  AND  PROBABILISTIC  INFORMATION 


v  =  820  ft/s 

Longitudinal  Model 

a.  Dimensional  De 

rivatives 

Xu  = 

0.015 

1/s 

Xw  = 

0.004 

1/s 

Zu  = 

-0.074 

1/s 

Zw  = 

-0.0806 

1/s 

Mu  = 

-0.0786 

1/s-ft 

Mw  = 

-0.0111 

1/s-ft 

Mq  = 

-0.924 

1/s-rad 

Mw  = 

-0.00051 

1/ft 

Distrubance  Noise  Standard  Deviation 


a    =  a    =1.105  1/s  (10  ft/s)2 
a  =  30.0  1/s  (10  ft/s)2 


(7  ft/s  rms  gust  with  a  930-ft  correlation  distance) 


c.  Observation  Noise  Standard  Deviation 

a     =  0.15  s  (0.01  rad/s)2 
q 

ah  =  0.05  s  (100  ft)2 
ad  =  30.0  s  (10  ft)2 
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Lateral  Models 


Dimensional  Derivatives 


Yv  =  -0.0868  1/s 

N'  =  2.14   1/s 
p 

H'r  =   -0.228  1/s 

N'  =  -0.0204  1/s 

L'  =  -4.41   1/s2 
p 

L'  =  0.334  1/s 
r 

L'  =  -1.181  1/s 


Disturbance  Noise  Standard  Deviation 

a  =  1.63  x  10"4  1/s 

(7  ft/s  rms  gust  with  a  930-ft  correlation  distance) 


c.  Observation  Noise  Standard  Deviation 
a  =  1.5  x  10"5  s 
a,  =  1.5  x  10"5  1/s 
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APPENDIX  C 
AN  AID  TO  USING  OPTSYS  AT  NPS 


I.   INTRODUCTION 


One  of  the  tasks  involved  in  my  thesis  work  at  the  Naval  Postgraduate 
School  (NPS)  was  to  verify  some  of  the  data  of  reference  [1]  which  in- 
vestigated the  sensitivity  of  the  Steady-State  Kalman  Filters  as  lateral 
and  longitudinal  estimators  in  Strapdown  Inertial  Navigation  Systems 
(INS).  One  of  the  recurring,  essential  calculations  was  for  the  steady- 
state  gains  of  each  system  model  considered.  Fortunately,  the  OPTSYS 
computer  program  was  available  in  Fortran  at  the  computer  center  to  help 
perform  this  enormous  job.  The  use  of  the  OPTSYS  program  was  covered  by 
reference  [2],  but  not  in  adequate  detail  for  easy  application.  After 
much  trial -and-error,  frustration,  attempted  decoding  with  the  assistance 
of  the  computer  center  staff,  and  prayer,  and  at  the  expense  of  many 
man-hours  of  time,  our  Lord  enabled  me  to  properly  fill  out  and  order  the 
data  cards  for  a  particular  modeled  system  and  obtain  the  expected 
results  upon  execution  of  the  program.  Since  Professor  Collins  has 
several  other  students  in  need  of  a  users  working  knowledge  of  the  OPTSYS 
program  and  anyone  using  Kalman  Filters  can  benefit  as  well,  I  am  writing 
a  more  detailed  description  of  how  to  correctly  input  data  by  discussing 
a  specific  example.  The  intent  of  this  paper  is  to  supplement  the 
guidance  of  reference  [2]  and  further  facilitate  research  at  NPS. 
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II.  MODEL  AND  ESTIMATION 

Consider  the  linear  time-invariant  system  given  by 

x  =  Fx  +  Tw 
z  =  Hx  +  v 

where  x  represents  the  states  of  the  system;  z  is  the  measurement  vector; 
F  is  the  system  matrix;  r  is  the  driving  noise  coefficient  matrix;  H  is 
the  measurement  scaling  matrix;  and  w  and  v  are  independent,  zero-mean, 
white  gaussian  noise  processes  with  covariance  matrices  Q  and  R, 
respectively. 

A  continuous  time  Kalman  Filter  for  this  system  is  described  by 


x  =  Fx  +  K(z  -  Hx) 


where  x  is  the  state  estimate  and  K  is  the  matrix  of  the  steady-state 
gains  of  the  Kalman  Filter.  The  implementation  of  the  System  Model  and 
the  Kalman  Filter  are  shown  below  in  Figure  C-l  [Ref.  1]. 
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MATHEMATICAL  MODEL 
SYSTEM 


MEASUREMENT 


w- 


/ 


H 


^OWr 


KALMAN  FILTER 


>— 

K 

/ 

F 

- 

H 

Figure  C-l.   System  Model  and  Kalman  Filter 
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III.  AN  EXAMPLE  OF  LONGITUDINAL  MOTION  ESTIMATION 

After  state  vector  augmentation,  the  resultant  model  of  longitudinal 
motion  of  an  aircraft  of  the  form  x  =  Fx  +  Tw  is 
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-0.806 

w 

-10.7 
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h 

0 

ug 
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where  the  units  are  scaled  such  that  u,  w,  u  ,  and  w  must  be  multiplied 

y      y 

by  10  to  give  feet  per  second,  q  by  0.01  to  give  radians  per  second,  9  by 
0.01  to  give  radians,  and  h  by  100  to  give  units  of  feet  [Ref.  1]. 
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The  corresponding  measurement  model  in  the  form  z  =  Hx  +  Iv  is  given 


by 
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(26) 


For  this  model  Qu  =  Qw  =  "1-105  (10  ft/s)2/s,  R  =  0.15  (0.01  rad/s)2  and 


Rh  =  0.05  (100  ft)2  s  [Ref.  3] 
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IV.  APPLYING  OPTSYS  TO  THE  EXAMPLE 

The  essential  input  data  that  will  enable  OPTSYS  to  calculate  the 
steady-state  gains  of  the  Kalman  Filter  and  many  other  parameters  out- 
lined in  [Ref.  2]  follows  on  page  55.  The  input  data  and  control  cards 
are  described  in  the  paragraphs  below. 

Card  1  -  The  17  entries  in  every  other  column  from  column  2  through 
column  34  essentially  tell  OPTSYS  what  to  compute.  See  [Ref.  2]  for  more 
details. 

Card  2  -  The  5  entries  in  every  third  column  from  3  through  15  de- 
scribe the  system  being  modeled  to  OPTSYS.  The  first  entry  tells  the 
number  of  states  or  order  of  the  system-7  since  there  are  seven  rows  in 
the  F  matrix.  The  second  entry  gives  the  number  of  control s-0  since  u=0. 
The  third  entry  tells  that  we  have  2  measurements,  while  the  fourth  entry 
shows  that  two  process  noise  sources  exist.  The  fifth  entry  is  always 
zero  when  filter  synthesis  is  done.  See  [Ref.  2]  if  regulator  synthesis 
only  is  desired. 

Cards  3-16  -  These  cards  contain  the  F  matrix.  The  first  six  entries 
of  each  row  go  on  one  card  with  12  columns  for  each  entry-1-12,  13-24, 
....,  60-72.  The  seventh  entry  for  each  row  is  placed  in  columns  1-12  of 
a  continuation  card  that  immediately  follows  the  card  with  the  first  six 
entries  of  the  row.  Note  that  if  our  example  system  were  6x6,  the  F 
matrix  would  only  take  up  cards  3-8. 
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The  next  three  cards,  17-19  in  our  example,  contain  the  H  matrix. 
Note  that  this  matrix  is  also  entered  on  the  cards  by  rows,  but  consecu- 
tively with  an  entry  in  every  12  columns  with  6  entries  per  card  as  long 
as  unused  row  elements  remain!  Thus  the  first  entry  of  row  2  of  the  H 
matrix  appears  in  columns  13-24  of  card  18. 

The  next  three  cards,  20-22,  hold  the  T  matrix.  This  matrix  is  also 
entered  consecutively  by  rows  with  an  entry  in  the  first  14  groups  of  12 
columns  on  the  cards! 

The  next  to  the  last  card  gives  the  Q  matrix.  Note  that  this  card 
has  only  the  diagonal  terms  of  the  matrix  in  columns  1-12  and  13-24.  See 
[Ref.  2]  for  matrices  with  non-diagonal  terms. 

The  last  card  is  for  the  R  matrix  and  also  has  diagonal  entries  in 
columns  1-12  and  13-24.  Again  refer  to  [Ref.  2]  if  non-diagonal  terms 
exist. 

This  supplement  will  be  effective  until  the  OPTSYS  program  is 
re-coded  in  WATFIV  language.  Its  usage  should  greatly  improve  the 
efficienty  and  morale  of  those  using  the  OPTSYS  program  on  file  at  NPS 
Computer  Center. 
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COMPUTER  OUTPUTS 


C/OPTSYS1     JOB     MU30,  0417)  ,•  POTTEE     G.G.' 
C/    EXEC    PCRTCLG 
C/STSIN    DC    * 

IMPLICIT    REALMS  (A-H.O-Z) 

DIMENSION    ACL  Mb,  16)  ,  B(B  .3)   ,BA(16 .16)  ,CI (16) ,CB ( 16) . CO (16 , 16) 
^CWI  (16)  ,  CHE  (16)  .PBGC  (8, 1b)  ,  F3GE  (16.  8),  G  (16.  16).  3(1  (1o,  16). 

»B21  {16',  16j  ;  X(32',32J  .GfJ(l6,T6)  ,Hd  (8',  H  ) "'  D1'(  32)  ,  D2  (  3  2)  '.  HH  (32  ,  32  ) 
i»Q  (3  f  8)  ,GM  (16,3) 


,9)  ,SC  (16,16)  ,HB  (3  2)  .»I  (32)  ,K1  1  (16,  16    , 
,32)  .GN  (16,1  6)  ,HO  (8, 15)  .01(32)  ,02(32)  .R.I 
.u,  ^)  ,HN0RHJ16,1  6)  ,  .'NORM  I  Mb  ,  16)  . DESTAB  (16)  , 
CAA(16.16),3HM6,8)  ,C8  (8,  16)  ,fc  (8,  81,0570  &E  (16,  161  , 
-Z?  (32)  ,RES  (32)  .AY  (6) 
C(8\32[,KU(8,32) 
)U  I  VALENCE     (t1  1  (1,  1)  ,GH  M  .  1)  )  ,    (W  1  1  (1  ,  1 )  ,  G  V  ( 1  ,  1)  ) 


CAA(16,16),BR(16,8)  ,CH  (8.  16)  ,0  (8,8)  ,OSXOaE  (16,  16)  , 

'•OCF  (3  2)  -Pis  (3  2)  .AY  (6)  ,  BB  (3  2  )  ,  CC  (  32)  ,CP(16)  ,  GW  (  32  ,  8)  ,  G  V  (  32  ,  8) 
SHY(8, 32  f ,H0 (8,32) 

EQUIVALENCE     ( W  1  1  (1  ,  1)  ,GH  (1  .  1)  )  ,    (W  1  1  (1  ,  1 )  ,  C  " 
1       (-21(1,1)  .HYjI.lh,   (W21(1  ,1    ,HU(1,1)) 

COMf.ON    /PROG/    IOL,INC,IQ,IP.  ,ISS,IM,ir?1  ,  IT 
"      lOSTAE,! DEBUS, IS2T,IBEG,i:  PSD,  iyU,INOBH 


[TF2, ITF3 , IFDFW,IE, 

DATA    STAE/'^V 
U     FORMAT(40I2) 
101     READ(5-U,END=100)     I0L,IQ,INQ,IH,ISS,IM,ITF1, ITF2 , I TF 3 , I FDFW, I E, 
*  IDSTA6.IDE3UG,ISET,IPSD.IYU,IN0RM 

R£A0(5,7U32)     NS , NC, NO B, NG, I  REG 
7U32    FORHAT(5I3) 

WRITE  (6,U000)     NS,NC,N0B,N3 
UOOO    F03MAT(M«  ,  2X  , '  ORDER    OF    SYSTEM    =  •     I  3,  //  ,  2X  ,  '  NUMB  ER    OF    CONTROLS    =' 

1  ,     13,//, 2X.' NUMBER    OF    OBSERVATIONS    =',I3,//,2X, 

2  'NUMBER    OF    PROCESS    NOISE    SOURCES    =«,I3,//f 
N2=2^NS 

CALL     INNER  (NS,NC, NOB,  NG, N2 , ACL, B, 3 A , CI , CS, CO, CHI , CHR , D, FE>GC,FBGE, 
OG,GAM,GM,GN,H0,D1,D2, PBO.Bfl , RC. 0, SC, HR, HI, W 1 1 , H2 1 , X, 
*»S0SH,WNORni,DESTAB,AA,BH,CH,JCP,BES# AY, BB ,CC, CP, GH, GV, HY  ,  HO, 


*    DSTOR 

lA 

)S 

GOTO    99 


IE) 

99    READ(S,5,EXD=100)     S  TA 
FOR  MAT  (A2) 
IF(STAR.EQ.STA)        GOTO     101 


100    STDF 
END 

SUBFOUTINE     SETU P (3A ,G ,GA M, N S , NC , NG) 
R  FT  U  S  N 
END 
SUBFOUTINE 

INNER (NS,NC. N0.NG.K2.  ACL,B, BA.CI  .CP.CQ.CWI    CHR, D,  FBGC,?BGE, 


GV, HY.HU, 


n  (NS.NS)  ,W2^  (NS,  NS)  ,  X(N2,N2l    ,GNjNSf  NSI  ,H0  (N0.N3)  ,  6l  (N2)  ,D2  fN2)   -RM( 
n2,N2)  ,Q(NG  ,  NG)  ,  D(NO,NC)     ,  3A  M(NS  ,  NG)  . H NORM { NS , NS)  , HNO RMI (NS, H5 J 
"       ,  DESTAB(NS)  ,  A  A  (NS  ,NS)  ,  BM  (  NS  ,NC)  ,CM(NO,NS)  ,  JCF  (N2)  ,  F.ES  (N2J  .  A  I  (NO 
f  .DB(N2),CC(N2)  ,CP  (NS)  ,  J  H (N2 ,NG)  ,  J  V  (N2,N0)  , HY (N0,N2)  ,  HU  (NC,N2) 

*  .  nSTORE  (NS  .fiSl 


*■  INNER  (NS,NC,  N0,NG,K2,  ACL,B,  BA.CI  ,  CP  ,  CQ  ,  CW  I  ,  CHR, 

»G,J  AM,GH,C,  N,H0,D1,D2,PF-3,SM,RC,  6,  SC,  HB,  WI.H1 1  ,  H2  1  ,  X, 
•-•WN3  R  M  ,  H  N  OR  M I  ,  DE  ST  A  E  ,  A  A  ,  B  M ,  C  M  ,  JC  F  ,  P  SS ,  A  Y  ,  H  B  ,  CC  ,  CP  ,  G  U  , 
C    DSTORF) 

IMPLICIT    ItEAL"8  (A-H,0-2) 

DIMFNSION    ACL(NS",NS)  ,  B  (N  C,  N  C)  ,  B  A  (NS  ,  N  S)  ,  CI  ( NS)  ,CR  (NS)  ,Cg(NS,NS) 
"CHI  (NS)  ,  CHR  (N3)  ,  P8GC(  NC  ,  NS  I    ,FbGE    NS,NO|  ,G  (NS,  NS)  ,  GM  (NS,  No)  , 
e  PRO(NS.NS)  ,  RC(NO,  NO)   ,SC  (NS,NS)  ,HR  (N2)  ,  HI  (N2)   ,  HI  1 

NS)  ,  W2  1  (NS,  NS)  tX(N2,N2|^  f  GNJ  NS it  NS|  f  HO  ^NO  ,  NT 

ESTAB(NS)  , 
.£BfN2),tC 

.DSTORE  (SS,f)S) 

COMrON    /PROG/    IOL, INC ,IO.TR  ,ISS, IM, ITP1 ,  ITF2.ITP3 ,IFDPH, 
"  IDSTAB,ID£3UG,ISt?,IRE3  ,IPSD,  IYU,  INOBB 

FEAL"U    F-.T  (20) 
NSQ^NS^NS 
C 
C*  s*OUTPUT    OPTIONS 

C ICL=1     IF    THE    OPEN    LOOP    EIGENSYSTEM     IS    DESIRED — OTHEFHISE    IOL=0 

IF    THE    RMS    VALUES    OF    THE     CONTROL    AND    STATE    ARE    TO     BE    FO'J'ID 
IF    ONLY     B    AND    P     ARE    DIAGONAL 
IP    A,     b,    Q,     AND    R     APS    DIAGONAL 
IF    OPTIMAL    FILTER     AND    FEGMLA703    EI GE N SY STEMS    ARE    TO    5E    FOUND 
IP    EXTERNAL    C    MATFIX    IS     SUPPLIED 
IF    EXTERNAL    K    IS    SUPPLIED 
IP    EXTERNAL    C    AND    K    ARE     SUPPLIED 

IF    STEADY    STATE    VALUES     APE    TO    3E    DETERMINED 
IF     MODAL    STATES    DESIPED 


c- 

--10=1 

c- 

-- :nq«i 

c 

INy=0 

c- 

--I F=0 

c 

IP=1 

c 

IF  =  2 

c 

IF=r3 

c- 

--ISS  =  1 

c 

IM=1 

c* 

>  nr- 

c 
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3 
7444 
6000 
6001 
6003 
60  10 
601  1 
6051 
6052 
60  12 
6313 
8515 


FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

FOR 

MH 

M    = 


pat 

r.AT 

PAT 
MAT 
HIT 
.".  A  T 

r.AT 

MAT 
LAT 
r.AT 
P.AT 

rAT 

=     N. 
N2 


-•OPEN    LOO?    DYNAMICS    KATRIX.. 
6E12.S) 


,//) 


10 (2X,  1PD10.3)  ,/.2X,  10  (2X,  1PD10.  3)  ) 

•0"  ,//,2X, 'THE  CONTROL  DISTRIBUTION  MATRIX ',//) 

■ 0« ,//,2X, 'THE  CONTROL  WEIGHTING  MATRIX '  , // 

0'  ,//,2  X,  'PROCESS  NOISE  DISTRIBUTION  MATRIX './/) 

0' ,//,2X, 'POWEB  SPECTRAL  DENSITY  -  PROCESS  NOISE .',//) 

0'  ,//,2X,  'MEASUREMENT  SCALING  MATRIX • , //) 

0» ,/, 2X  ' POWEE  SPECTRAL  DENSITY  -  MEASUREMENT  NOISE..',// 
0' ,//,2X, 'DIAGONAL  OUTPUT  COST  MATRIX...',//) 


0'  ,//,2X, 


0'  ,//',2X,  'OUTPUT    3DST    MATRIX...'     //) 

'"",  'MEASUREMENT    FEEDTHROUGH    MATRIX...',//) 


SUEROUTINE    CHECK    CHECKS    THE    CONSISTENCY    DF    REQUESTED    OPTIONS 


9010 


9014 

9015 
9016 


5001 


CAL 

IF( 

DO 

REA 

IF( 

REA 

CON 

GO 

CAL 

CON 

WRI 

DO 

WRI 


(  if  b,   H«_,  N<j,   NUJ 

.  1)  GO    TO    90  15 

1         H  S 

4f     (BA  (I, J)  ,J=1.NS) 

.  EQ.     0)     GO    TO    901 4 
4)      (DESTAB(I)  ,1=1  ,NS) 


495 
096 


497 


in 

IF 
DO 
IF 

WRI 
FOR 

CON 

IF( 

DO 
DO 
VII 
CAL 


L    CHECK (EPS, NC, NG. NO) 

ISET.Ei 

9010     1= 

D(5,744 

1DSTAB 

D(5,744 

TINUE 

TO    9016 

L    SETUP  (3A,G, GAM, NS,NG,NC) 

TINUE 

IE  (6,3) 

5001    1= 

(BA  (I.J) #J»1.  NS) 
.    0)     GO    TO    3999 

STABILIZATION     CASE '     /, 

OWING  VALUES  WILL  BE  ADDED  DOWN  THE  DIAGONAL  TO  ', 
E  THE'./,'  ABOVE  MATRIX.  OPTIMAL  GAINS  FOR  THE  ', 
ED    SYSTEM    ARE    THEN     USED'     /, 

SUBOPTTMAL    GAINS    WITH    THE    ABOVE    SYSTEM',/) 

(DESTAB  (I)  ,1=  1,NS) 

OPEN  LOO?  DYNAMICS 
IQ.EO.3)  GO  TO  500 
NC.NE.3)     GO    TO    503 

,GN, LOW.IH IGH, D1) 

, LOW ,IHIGH ,GN, D2) 

. LOW ,IHIJH ,GN, D2.SC) 

OW.IHIGH.GN.CWR.CWI.SC,  I  ERR) 

ALL    EREXIT  (NS,GN,IERR) 

.LOK.IHI3H.D1.  MS, SCI 

ZE    AND    PRINT    OPEN    LOOP    EIGENSYSTEM 

ITE    =    1 

L    CSOFM  (CWR,CWI, SC,NS,IW  RITE ,  NS3,DDD, 01, D2 , WNGRM, WNOPM I , HO , CM , 

NO.NS) 
IOL.EQ.2)     RETURN 
(10. EQ. O.OR.  (NC.NE  .O.OR.  IDSTAB.GT. 0)  )     GO    70    S00 

496  7=1, IIS 

(CWR  (I)  .  LT.O.)     GO    TO    49S 
TF     (6,u95) 

MAT     (///'     FROGRAM    TERMINATING    DUE    TO    UNSTABLE    SYSTEM') 
UPN 
TINUE 
IOL    .  EO.    3)     GO    TO    510 

497  I  =1  ,  HS 


497    J=1  ,  MS 
(I, J)  =SC  fl.Jl 

L    BINV     (>»3Q.ll11,IIS,rOD,Dl,D2) 
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500 


505 


506 

507 
510 


1806 

8520 
8S0U 


8517 
8519 


9510 

9210 

8500 
9120 
9020 

9505 
9215 


9216 
9217 


9218 
9219 


9508 
9030 
9035 

9520 
90U0 


CON 
IF_( 

DO 
DO 
AA  ( 
DO 

DO 

D 

D 

D 

DST 

8A  ( 

CON 

REA 

WRI 

DO 

WRI 

IF( 

CAL 

CON 


TINOE 

IDSTAB    .EQ.     0)     GO    TO    510 

F0R;1     g    y    DIAG(DSSTAB)     *    0-IN1 

505    J=1,NS 

505    1=1, NS 

I.J)     =    WNORM  (I,  J)  >DESTAB  (J) 

507    1=1, NS 

507    J=1,NS 

DD    =    0.  DO 

0    506    K=1,NS 

DD    ■    DDD    ♦    AA  (I,  K)  ^WNOP.MI  (K,  J) 


ORE(I,J)     =DDD 

I, J)     =    BA(I,J)      ♦    DDD 

TINUE 


D(Se7UU«)      ((HO  (I, J)  ,J  =  1,  NS),I=1,ND) 

TE  fb,6051) 

1806    I    =    1,NO 

TE(6,6000)      (HO  (I.J)  ,J=1  ,  NS) 

IH.NE.1)     GO    TO    350« 

L    rtODE(WNORH,HO,C.1,NS  ,NO  ,NS,2) 

TINUE 


IFflFDFW    .EQ.     0)     GO    TO    8519 
READj5,7UUU)      ((b(I,J)   ,J=1,NC)  ,I=1,NO) 
WRITE  (b,85l5) 


WRI 

DO 

WRI 

CON 

NOB 

IF 

IF 

IF 

IF 

RE 

CON 

REA 

WRI 

DO 

WRI 

IF( 

CAL 

CON 

GOT 

DO 

DO 

pnr 

IF 

REA 

DO 

DO 

AA  ( 

GO 

PEA 

DO 

DO 

DDD 
D 
D 

AA( 

WRI 

DO 

WRI 

DO 

DO 

DO 

!?f 

PEA 

IPf 

CON 

DO 

DO 

3(1 


120 


E  (6,8515 

6517    T=1.NO 

TE(6,6000)      (D  (I,J)  ,J=  1,NC) 

TINUE 

=    0 
(NC    .EQ.     0)     GO    TO     1801 
IOL    .EQ.     3)     GO    TO    9035 
I3.NE.1  .  AND.  IR.NE.  3)     GOTO    91 
1SET.  EQ.  1)  GO    TO     95  10 

P (5,7UUU)      ((G  (I,  J)  ,J    =    1  -NQ   ,1  =  1,  NS) 
TINUE 

P(S,7UH4)      ((FBGC  (I  ,J)  ,J=  1,NS)  ,1=1,  NC) 
TE  (6  .6001) 

9  21 6    1  =  1, NS 
TE  (6,6000)      (G  (I.J)  ,J=1,NC) 
IH.NE.1)     GO    TO    §506 
L    nODE(V;!OR.1I,G,  B.1,NS,NS  ,  NC,  0) 
TINUE 
0    6399 
9020    T=1,NS 
9020    J=1,NS 


I  +  flH,  J]  =6.  0 

INQ    . EQ.     1)     GO    TO    9215 
0  (5,7um 


0  (5.7U4U)      (AY  (I)   ,1  =  1,  NO) 

95  05    1=1, NO 

^505    J=1,NS 

I.  J)  =     HO  (I, J)  "AT  (I) 

TO    921" 

DI5.7WU)      {(Q(I,J)  ,J=1,NO)  ,1-1, NO) 

92  17    1=1  ,NO 

^217    J=1,NS 

=    0.D0 
O     9216    K=1 ,NO 
DD    =     DDD    ♦    Q  (I,K)  "  HO(K,  J) 
1,J)     =    DDD 
TE  (6.6013) 
9218     1=1. NO 

TEfb,6000)      (Q  (I, J)   ,J  =  1,MO) 
9508     1=1, NS 


^508    J=1,NS 

9508     r=1,NO 

I*BH.  J)  =R3  (I*.1H,  J)     ♦    AA  (K,I)  *»0(K,  J) 

ISET.  FQ.  1)  GO    TO    95  20 

Djf5,7«uu)      ((G(I.J).J    =    1  ,SC)   ,1  =  1, NS) 

TOL    . EQ.     3)     GO    TO    90U 5 


TINUE 
Q0U0  1=1, NC 
9GU0  J=1,NC 
,J)=O.0 
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READ(5,7I44U)  (BII,I1,I*1.HC) 

IFjINQ  -EQ.  1)  GO  TO  9015 
WRITE  (6,6012) 

WRITE  (6,6000)   (AY(I) ,1=1 ,NO) 
90U5  WRITE  ?6, 6001) 

DO    606    I    =     1,NS 

606  WRITE  (6,6000)      (G(I,J),J    =    1,NQ 
IF(IS.NE.I)     GO    TO    650  1 

CALL    MODE (tf NORM I, G, BM,NS,NS ,NC,  0) 
85  01     CONTINUE 

IF(IOL    .EQ.    3)     GO    TO    8505 
WRITE  (6,6003) 
DO    b07    1=1, NC 

607  WRITE  (6,6000)      (  B  (I  ,  J)  ,  J=  1 ,  N  C) 
8399    IF(ITF1     .EQ.    0)     GO    TO    8<*0& 

C 

C       OPEN    LOOP    TRANSFER     FUNCTIONS 

C 


8505  WRITE  (6,9220) 
9220  FORMAT(' 0«  ,//,2! 


!X,'OPEN  LOOP  TRANSFER  FUNCTIONS ') 

ITFX=1  " 

CALL    TF(NS,NS#NSQ,BA,AA,N:.G,BM,NO,HD,Cfl,IFDFV,D,3B,CC,CP, 
*  WR, WI.CWR,CWI,SC, JCF.RES,D1,  D2 , DD D , EPS , ITF1 , I TFX) 

8U00    IF(IOL    -NE.    3)     GO    TO    8502 
IF(NG    .  EQ.     0)     RETURN 
GO    TO    6  25 
8502    CONTINUE 

IP(IR    .  EQ.     1    -OR.     IR    .EQ.     3)     GO    TO    9130 

C*»*CALCULATION    OF    CONTROL    GAI NS :  FORK  ATION    OF    CONTROL    HAMILTONIAH 

C 

C         T  <•>  9>>F    AND    FT    APE    THE    OPEN    LOOP 

C          O  *                                    DYNAMICS     MATRIX     AND    TRANSPOSE 

C         »  F                        -GM«"BI^GnT»'  0»*BI     IS     NCXNC    CONTROL    WEIGHTING 

C         0  *                                    MATRIX 

C         *  *  ***K     IS    THE    NSXNS    STATE    WEIGHTIN 

CO  *                                    MATRIX 

C         *>  -A                                -FT          * 

C          <**»  a*  ***GM     IS    THE    NSXNC    CONTROL 

C  DISTRIBUTION     MATRIX 

DO  2U  I  =  1,NC 
DO  2*  J  =  1 ,MH 
2U  PR0fI,J)  =  6  (J,I)/B(I,I) 
DO  25  I  =  1,MH 
DO  25  J  =  1.MH 
Rf1(I,  J*MH)  =0.  DO 
DO    25    K    =    1,HC 

25  Rfl(I,J*MH|      =    RM(IfJ*KHl     -    3  (T , K)  *  PRO  (  K,  J) 
C*»"2NX2N     HAMILTONIAN    M  ATRI  H**'***  "^"'c  «o»*  »  *i 

C DIAGONAL    BLOCKS .'.11     AND    K22 

DO    26    I     *    1,MH 

DO    26    J     =    1.MH 

fintl,  J)     =       BA  (I, J) 

Rflfl  +  flH.JtHH)     =    -BA(J,I) 
C (121    BLOCK 

26  FH(I*NH,J)     »-RH(I*HH,  J) 

C M12    BLOCK     IS     DEFINED     IN    LINE     25    ABOVE 

5  0    CONTINUE 

I?(IDEPUG    .EQ.    0)     GO    TO     1050 
WRITE  (6  ,  60  1U) 

FOPrAT(//,'     EULEP-LAGRANGE     SYSTEM    MATRIX ',//) 

CALL    RAPRNT  (M  r.1 ,H,9,RM,U     •  ( 9( IX.  1PD13.6)  )  •) 
CALL    EALANC(M,M,  FM,  LO  W.I  HI  3  H,  D1 ) 
CALL    ORIHSS  (M  ,  M  ,  LOW  ,  I  HIG  H,  a  M,  02) 


601U 

FOP»AT(//,  • 

CALL    RAPRNT 

1050 

CALL    BALANC 

CALL    ORIHSS 

CALL    ORTFAN 

CALL  HQR2(ft 
IF(IERR     .HE 

AN  (H,»  .LOW,  I  HIGH,  R  M,  D2  ,  X) 
(fi.M.LOW  ,  I  HIGH,  r.M,  WR,W  I,X.  I  ERR) 
NE.     5)     CALL    EREXIr  (H.Rn.XESR) 


CALL     SALFAK(M,T.  ,LOW,IHIGH,D1,M,  i) 
C    DE5DG    DIAGNOSTICS    OS    E-L     EQ 


60 


IF(IDEBUG    .EQ.     0)     GO    TO    53 
WRITE  (6,91  15) 
9115    FORMATf//'     EIGVAL    AND    EIGNVEC    OF    2*N    E-L    EQ.     AFTER    HQR2'//) 

nr\      CO       T  —  1       m 


DO    52    1=1. a 

52    WRITS  (6,91  16)     »R(I),WI(I) 

116  FORMATMX.  1P2D13.6) 
WRITE  (6.  91  17) 

117  FOR  MAT  ('0') 
\PRNT  (M,M  ,M,9,X,4,  •  (9  (1X,  1PD13.  6)  )  •) 


CALL    PA_ 
5  3    CONTINUE 

IF(IDSTAB  . 

IF  (NCB.EQ.O 

IF  (NOB.NE.O 
5U  IF  (NOB.NE 


9119    FOR  MM  MO'  ,//,2X 
9121 


Q.  1)  GO  TO  5U 

)  WRITE  (6  ,91  19) 

j  WRITEJ6  ,91211 


)     GO    TO    60 


F0RMAT(' 0'  ,//,2X,  ■  EIG  ENSYST  EM    OF    OPTICAL    CLOSED    LOOP     SYSTEM..1,// 

FORMAT?' 0«  ,//,2X, 'EIGENSYSTEM    OF    ESTIMATE    ERROR    EQUATION " , // 

CALL     RGAI N(M, NS,NC,NO  3, WR,WI,  X,  GN,W1  1,  RM, 


CALL     Rg£i  N(M,  NS,NC,N0  3,  WR,WI,X,  GN,W1  1,RM, 
1W21  ,D1,CWR,CWI,SC,MHS,D2) 
...CK    EIGVEC 

IF(IDE5UG    -EQ.    0)     GO    TO    753 
WRITE  fo, 9125) 
9125    FORMAT(*     EIGENVECTORS     FROM     RGAIN    PRIOR    TO    CNORfl'l 
CALL    FA?RNT(NS,NS,NS,9,SC,4  ,  •  (9(1X,1PD13.6))  ') 


750    CONTINUE 
C 

C    RESET    FLAG    AND    F    MATRIX    FOR    ITERATIVE    DES T ABIL IZ ATION    CASE 
C 

IF(IDSTAB    .EQ.     0)     GO    TO    9136 

DO    9135    1=1. NS 

9135  3A(I,I)     =    BA(I,I)     -    DESTAB(I) 
IR=1 

9136  CONTINUE 

C       CALCULATION    OF    FEEDBACK    GAIN 
C 

Ct*»FEFDBACK    GAINS 0    =    -  (  BIN  VER  SE)  *  GT^GN 

C CALCULATE    GT 

DO    80  1    I    =     1,NC 

DO    801    J    =     1,  NS 

PFO  (I.J)     =     0.  DO 


DO    80  0    K    =     l.fiH 

00  PRO  (I.J)     =     PftO(I,J)     ♦G(K,I)  ^GN(K.J) 

01  F3GC(I.J»     =    -?HO(I,J)  /S(I,I) 
IF(IDSTAB    -EQ.     1)     GO    TO    9130 


NORMALIZE    AND    PRINT    OPT.     REG.     CLOSED    LOOP     EIGENSYSTEM 


IWRITE    =2 

CALL    CNORM  (CW R, CWI , SC , NS , I W PI IB , NSQ , DDD, D 1 , D2 , WNO RK, W NOP MI , FK GC, 
*>  AA.NC,N3) 

C55«THE    OTTIMUM    FEEDBACK    CONTROL     GAINS 
9130    WRITE  (6,977) 

977  FORMAT*///, '     '.'THE    CONTROL     GAINS    ARE:',//) 
DO    96  6    I    =     1,NC 

968     WRITE  (6.978)      <P3GC(I,J),J    *     1.NS) 

978  FORMATf*     '     2X , 1 P6 D 1U. 6, /, 2X  ,  6D1 U . 6) 
C    COMFUTE     MODAL    C    MATRIX 

C    OPEN    LOOP     D-INVE    SAVED     IN    WNOHMI 

IF  (I  P.    .  NE.     1)     GO    TO    9  85 
C    IN     COMPUTING    MODAL    C    RECOMPUTE     □    OPEN    LOOP 

C       SINCE     WNGRM    USED    TO    STORE    U     AND    U-INV     PO R     CLOSED    LOOP    SYSTEMS,     AND 
C       WNORMI     USED    TO    SAVE    U-INV    OPEN     LOOP 
C 

DO    8510    1=1, NS 

DO    85  10    J=1,NS 
85  10    WV0FM(I,J)      =    WNORMI  (I, J) 

CALL     HI  ;.V(NS0,«'NORM.  NS.DDD,   01, 02) 

CALL     MODE(WSORM,FBGC, AA, NS, NC,NS,3) 
985    CONTINUE 
(T'THE    CLOSED    LOOP    DYNAMICS    MATRIX 

DO    160    I    =     1,  NS 

00     160    J    «     1,35 

SUM  =    0.D0 
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DO    150    K    =     1,NC 
150     SUM=SUM  *G  (I,K)  >  FBGC  (K,J) 

160    ACL  (I, J)  =BA  (I, J)  +  SUM 

WRITE  /6,  1701 
170    FORHATMO*  -  'THE    CLOSED    LOO?     DYNAMICS    MATRIX     IS..',//) 

CALL    RAPRNT(MH,MK,MH,5,ACL,  U,M5(1X,1PD13.6))  •) 

IF(IP.NE.1  .  AND.IR.  NE.  3)     GOTO    1801 

DO    91U0     1=1, NS 

DO    91U0    J=1 ,NS 
9140    GN(I,  J)  =  ACL  (I, J) 

CALL    BALANC  (NS,  NS,GK,  LOW  ,IHIGH,  D1) 
CALL    OPTHES  (NS,  NS , LOW  , IH I3H ,GN, D2) 
CALL    ORTF.AN  (NS,  NS  .  LOW  -IHI3H.GN,  D2,SC| 
CALL    HQR2(NS, NS , LOS ,1  HI 3 H,  3 N ,CWR ,CWI ,  SC,IERR) 
IF(IERR     .NE.     0)     CALL    EREXI T  (NS,  GN , I ER R) 
CALL    EALBAK  (NS,  NS, LOW  ,IHI3  3  -01,  NS,SC) 
era  ac-cirtc*  eci'ucr.i  *r-  » t  &  *  «4nn*  i -c  *ae  »  A*  at-  *»  4  >  * 

C 

C    NORMALIZE    AND    PRINT    CLOSED    LOOP     SUBOPT.     BEG.     EIGENSYSTEM 
C 

I  WRITE    =3 

CALL    CNORM  (CW  R,CW  I  ,  SC  ,  NS  ,  I  I  RITE  ,  NSQ,  0  DD  ,  D  1  ,  D2  ,  WNORH, V NORMI , FB GC, 
«  AA.NC.NS) 

DO    9300     1=1, NS 
IF  (CWRII).  LT-0.  0)     GOTO    9303 
WRITE  (6,9310) 
9310    FORHAT(///,'  PROGRAM    TERMINATING    DOE    TO     UNSTABLE    CLOSED    LOOP 

OSYSTEM" ) 
RETURN 
930  0    CONTINUE 

XFfXQ.NE.11     GOTO    1801 
DO    9600     1=1, NS 
DO    9U00    J=1 ,NS 
9U00    W11  (I.J)  =SC  (I,J) 

CALL    MINV(NSQ,W1  1,  NS,  DDD,D1  ,D2) 
1801     NOB=NO 

IF(     NG    . EQ.     0)     RETURN 


625    IF(ISET.  Eg.  1)  '  GO    TO    6  30 
;(5-7UC4)        ( 


(GAM  (I, J)  ,J  =  1,NG)  ,1=1,  NS) 

TO    9C 
9070 


!Q.     3)     GO    TO    9060 

GOTO 


READ,.,-- 
630    CONTINUE 

IF(IOL    . 

IF(INO.NE.O)     G 

DO    9050     1=1, NG 

DO    9050    J=1,NG 
90  50    0.(1  -J)  =0.0 

REAL (5, 7uuu)      (Q  (1,1)  ,  1=1 ,N3) 

GOTO    906  0 
9070    REALj5-7«UU)         (  (Q  (I  ,  J  )  ,  J  =1  ,  NG)  ,  I  =  1 ,  N3  ) 
9060    WRITF  (6,6010) 

DO    626    I    =     1.NS 
626    WRITE  (6,6000)      (G  A  1  (I .  J)  ,  J=  1  ,  NG) 

1F(  IM.NE.  1)     GO    TO    8503 

CALL    MODE (W NOP MI, GAB, AA, HS , NS,NG, 1) 
e503    CONTINUE 


IF(TOL    .EO.     3)      RETURN 
WRITE  (6,  6u  1  1) 


1) 


'  I  '  w  i 

.  0))      GOTO    389 


DO    627    {    =     1 ,  NG 

627  WRITE  (6,  6000)      (0  (I  .  J)  ,  J=  1,  S  G) 

628  IF(  rlO.  FQ.  0)  .  ASD.  (Nf 
DO     378    I    =     1,NG 
DO    37  8    J    =     1,  NS 

PtO  (I, J)  =  0.  DO 
DO  37  8  K  =  1.NG 
378  PRO(I.J)  =  ?RO(I,J)  *Q  (I,F)  »GAM(J,K) 
DO  J79  I  =  1, S3 
DO  379  J  =  1, NS 
CQ(I.J)  =  o.do 
DO    379    K    =     1,NG 


379    CQ(I.J)     =    CQ(I,J)  -GAM  (T,  f.\  *  FBO(K,  J) 
9100    IFMREG    .EQ-     1)     GO    TO    8a0t 
^••CALCULATION    OF    FILTER    G  AI  HZ  :  FOPM  AT^ 


N    OF    ESTIMATION     HAMILTONIAN 
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c 

C  <*°  **  ***F    AND    FT    ARE    SAME    AS    FOR 

C  »  >  CONTROL    HAr.ILTOMAN 

C  •  F                          -GMftQOGKT»  0*»Q    is     NGXNG    STATE    DISTURBANCE 

C  n  >  RCOVARIANCE 

C  *  >  5(j»a     IS    NOXNO    MEASUREMENT    NOISE 

C">  •  BCDVARIANCE 

C  rt    -H0TftBINr'HO  -FT          *  ''>BO    IS    NOXNS    MEASUREMENT    M  ATRI 

C  **  **  ft««*3M    IS     NSXNG    STATE    DISTURBANCE 

C  DISTRIBUTION     MATRIX 

1805    DO    91 10    1=1 ,NO 
DO    9110    J=1,NO 
91  10    RC(I,  J)  =0.0 

READ(5.7U4U1      (RC  (I,  T)   ,1=1,  NO) 
WRITE  (6,6052) 
DO     1807     I    =    1,NO 
1607    WRITE  (6  ,6000)      ( RC  (I , J )  .  J=  1 ,  NO) 
8506    IF(ITF2    .EQ.    0)     GO    TO    28 
C 

C       NOISE    TRANSFER    FUNCTIONS 
C 

WRITE  (6,9230) 
9230    FORMAT('0'//,2X' NOISE    TRANSFER    FUNCTIONS     ' 
C  ,  "THROUGH    THE    CLOSED    LOOP    SYSTEM..') 

ITFX=2 
IZERO=0 

CALL    TF  (NS ,NS,NSQ, ACL . AA,NG  ,G AM , BM, NO .  HO,  CM . IZ ER 0 , D, BE , CC, CP, 
*  WR,  Wl.CWa  ,CWI,SC,  JCF,RES,  D1  ,  D2,DDD,  EPS,ITF2,  ITFX) 

IF(IREG     .EQ.     1)      RETURN 
28    CONTINUE 

IFflREG    .EQ.     1)     GO    TO    388 
IF(IR.LT.2)     GOTO    1811 

READ(5,7UUU)      ((FBGE(I,J) ,J=1,N0) ,1=1, NS) 
GOTO    9320 
1811     CONTINUE 
C***     THE    MEASUREMENT    MATRIX 
C  »  (HOT-RIN'%HO==  =  >SC 
DO    30    I     =     1,NO 
DO    30    J    =    1,MH 

30  PB3II.J)     =     HO  (I, J) /RC  (1,1) 
DO    31     I     =    1,MH 

DO    31    J     =     1,MH 
RM(I  +  HH,  J)     =    0.  DO 
DO    31    K     =     1,NO 

31  RH(I»MH,J)      =    RM(I*HH,J)     -    II  0  (K,  I)  •  PRO  (K  ,  J) 
C^'GM^O*  GMT=  =  =>CC 

DO    3  5    I    =    1,NS 
DO    35    J    =    1.NS 
BM(I.J)  =BA  (t,  J) 
RM(I*MH.  J*MH)  =-3A  (J, I) 
3  5    EM(I.J*NS)     =CQ(I,J) 
GO    T6    50 
C-»^GC    BACK    TC    50    TO    SET    UP    THE    FILTER    HAMILTCVIAN 
C  AND   CALCULATE    THE    FILTER    GAINS 

60  CALL     RGAI  N(M, NS.NC.NOB,  WR,WI,X,  GN  ,GM     ,PM, 

1W21.D1.C8, CI, PRO    ,MHS,D2j 
C    CHECK    EI3 VEC 

IPflDEBUG    .EQ.     0)     GO    TO    58 
WRITEJ6.9125) 

CALL    nAr-PNT(NS,NS,NS,9,?F0,U,,(9(1X,1FD13.6))  ') 
58    CONTINUE 

IF(IDSTAB    .EQ.     1)     GO    TO    9311 
C 

C    NORMALIZE    AND    P5IHT    OPT.     ESTIMATOR    2IGSS5TSTEB 
C 

IWR ITE*U 

CALL    CNOPH  (CB,CI,?RO,  NS, IWfi IT E, SSQ, DDD, D 1 , D2, WNOr - , W NOP ■ I , HO,  A  A, 
»  fC.SS) 

93  1  1    DO    61     1=    1  ,MH 
DO    6  1    J  *    1  ,  NO 
61     PPO(I.J)     *     ♦HO(J.I)  /hC(J,J) 


63 


DO    6  2    I    =  1,3H 

DO    6  2    J    =  1 .NO 

FBGE(I.J)  =  0.D0 

DO    6  2    K    =  1  ,.1H 


6  2    FS3 
IF 
WR 
CALL 


cPRO(K,J) 


E(I,J)     =    FBGE(I,  J)  «-GN  (I.  K) 
(IDSTAB    .20.     1)     30    TO    9320 
ITE  (6.  1501) 
...LL    KAPRNT  (flH,.".H,HH,  5rGN,U  ,«  (5(1X,1PD13.6))') 
HRITEJ6,  15  10) 
DO    9  3  12    1= 1 , B H 
9312    X(I.I)     =    DSORT  (GN  (1,1)) 

WRITE  (6,  1520)      (X  (1,1)  ,1=1,  SH) 
9320    WHITEJ6.  1018J 

1018    FORMATf'O' . 'FILTER    STEADY    STATE    GAINS •  ,//) 

DO    63    I    =    1 , MH 

1019        (FEGE(I,J),J     =1,NO) 


63  WRITE  (6,  1019)  (FBGE(I,. 
019  FORflAT(»  ■  ,2X.1P6D1U.  6) 
COMFUTE    .MODAL    K    MATRIX 


1 

C    CO... 

C    OPEN    LOOP    U-INV    SAVED    IN    WNOR.1I 
IF(IM    .  NE.     1)     GO    TO    9330 
CALL    nODE(WNORMI,FBGE,  AA,3H,MH,  NO, '4) 
9330    CONTINUE 
C 

C    RESET    FLAG    AND     F    .MATRIX    FOR    ITERATIVE    D ES T ABILIZ ATION    CASE 
C 

IF(IDSTAB    .EQ.    0)     GO    TO    933  8 
DO    9335    1=1, NS 
DO    9  3  35    J=1,NS 
9335    BA(I,J)     =    BA  (I,  J)  -DSTORE  (I,  J) 

IR=2 
9338    CONTINUE 

DO  93U0  1=1, NS 
DO  93U0  J=1 ,NS 
SUI1=0.0 

DO    9350    K=1.NO 
9350    SUfl  =  SUn*FBGE(I,  K)  *HO(K,J) 
93U0    PRO  (I.J)  =BA  (I.J)  -SUA 


^jdu    sun  =  b  un»  r  ab  1 1  x.  i\j  ■ 
93U0    FRO  (I, J)  =BA  (I, J)  -I 

WRITE  (6,  936  1) 
9361    FOB  HAT  PO',' THE    CI 


yj-.-i.nz.    cLOSED    LOOP     FILTER     DYNAMICS     MATRIX     IS.  .',//) 
CALL    FAFRNT(NS.NS,NS,5.?R0,4,,(5(1X,1PD13.6))  •) 
IF(IR    .  LT.     2)     SO    TO    9500 

»»i»':r'.M»(>0>*Oi,>fin^>»*>f 


_F(IR    .  LT.     2)     G 

CA_. 

CALL    ORTHES (NS, NS , LO 


CALL    BALANC (NS, NS,PPO,LO W.I  HIGH, D1| 
'  ~W  ,IHI3H  .PRO, D2) 


IERR) 


C    NORMALIZE    AND    PRINT    SUBOPT.     ESTIMATOR    SI3EHSTSTEH 
C 

IWRITE=5 

CALL    CNOFH  (CR,CI,G.1,NS,IWPITE,NSQ,DDD,D1,  D2,  WNORM  ,  HSORSI,  HO,  A  A, 
*  NO.NS) 

DO    9« 10    1=1, NS 

IF(CR (I) -LT.0.0)     GOTO    9U 10 

WRITE  (6,9U20) 
9<*20    FORT.  A  .(////,*     PROGRAM    TERMINATING    DUE    TO    UNSTA3LE    FILTER') 

PETURN 
9U10    CONTINUE 

GO    TO    9501 
O500    IF(IC.EQ.O)     GO    TO    389 
9501    DO    651    =    1 ,NO 

DO    65    J    =     l.SH 

FRO<:,J)     =    6.  DO 

DO    65    K    =     1,NO 
65    FPO(I.J)     «     PRO(I,  J)  ♦RC(I,K)  *FB3E(J,K| 

DO    66     I     =     1,MH 

DO    6  6    J     =    l.SH 

CQ(I,J)     =    0.00 


64 


66 
388 


DO  66 
CQ(I, 
CONTI 
HE  P.. IS 
IB*  IB 
GOTO 
9380  DO  97 
DO  97 
X(I  .J 
DO  97 


K    =    1- NO 
J)     =    CQ(I,J)  -FBGE(X,K|  ^PRO  (K,J) 
NUE 

STATE    AND    CONTROL    RESPONSES 
♦  1 

(9360,9360,9380,9380),     IR 
05    1=1,  NS 
05    J=1,NG 


9705 


97  1  1 


9710 

1501 

1510 
9715 
1520 


9725 
9720 


9735 
9730 


9755 
975t> 


976  1 
9760 


9762 

9770 
9360 


Xrt.J 

DO  §7 
DO  9  7 
SU3=0 
DO  97 
SUM  =  S 
PRO  (Z 
PROJJ 
CQII, 

a«f 

H21    |j 

CALL 
CALL 
WRITE 
FORK  A 
CALL 
WRITE 
FORMA 
DO  97 

X  ^X.1 

WRITE 

FORT.  A 
IF  (I 
DO  97 
DO  97 
SUM=0 
DO  97 
SUM  *S 
X  (I.J 
DO  97 
DO    97 

sun=o 

IF      fN 

DO  97 
SUM  =  S 
PRO  (I 
CALL 
IF  (N 
DO  97 
DO  97 
W21  (l 
DO  97 
W21  (I 
DO  97 
DO  97 
3UM=0 
IF 
DC 
SUM  =  5 
TRO  (I 
DO  9  7 
DO  97 
PPO  (I 
PRO  (J 
CALL 
DC  97 
DO  9  7 
or.  (I, 
GHfJ, 
GOTO 
CALL 


I)?0'0 


c      v =  i      up 

|=X  (I.J|+GAB(I,K)»Q(Kt  J) 

10  J=I^NS 
.0 

11  K=1,NG 

UM-X(I  ,K)  »GAM  (J,  K) 
,Jj  =SUH*CQ(I,J) 
,15  =PRO(I,  J) 
J)  =SU« 

i  =sun 


,J    =GH    I, J 
,1     =GH]J,lJ 

niNV(NSQ,W21,NS,  DDD.D1  .  D2) 

SCOV     (HS.GH.W21.CS.  CI,  NS.GM.  W21,  CR , CI , PPO, 3 N) 
1501) 


"KE  ESTIMATION  ERROR',/) 


H) 


tt 


(6.  1501) 

T(' 0'  ,//,2X,  'THE    COVARIANCE    OF    THE     ESTIMATION    ERROR', //J 
RAFRNT(MH,MH,HH,5,GN,U,'  (5(1X,1PD13.6))  •) 
(6,  15  10) 

Tf'O'  ,/,2X,  •  RMS    VALUES     OF 
15    1=1, MH 

)     =    DSORTfGN  (1,1)) 
76,  1525)     jXfI,I)  ,1*1, S 
T(  (5(1X,  irD13.6|  I) 
Q.EQ.  0)     GO    TO    389 
20    1=1, NC 
20    J=1,NS 
.0 

25    K=1,NS 

UM+F&GC  (I,  K)  »GN(K,  J) 
)  =SUf1 
30    1=1, NS 
30    J=1,NS 
.0 

CEO.  0)     GO    TO    97  30 
35    K=1,NC 
UM*G  (I  ,K)  n  (K,  J) 

,j) =dg  (i, j) +SUH 

SCOV    Is'SjSC.WI  1,  CWP,Ca  I,NS  ,GM,W2  1  ,CR,CI,PPO,  BA) 

C  SO.  0)     GO    TO    97  56 

55    1=1, NC 

55    J=1.NS 

•  J)  =0.0 

55    K=1,NS 

,J)  =W2  1  (I,  J)  ♦PB3C(I,K)  ^BA(J,K) 

60    1=1  ,:)s 

60  J=1,NS 
.0 
CEO.  0)     GO    TO    9760 

61  K=1,NC 
UM*G(I  ,K)^W21  (K,  J) 
,J)  =du- 
6  2    I = 1 , H S 

62  J=I.NS 

,J)  =PRO(I,  J)  ♦CQ(I,J)»PRO(J,I) 
,1)  *PRO(I,J) 
SCOV     (NS,SC,H1 1,CVR,CW I,N3,SC,«1 1 , CWR , CV I , PRO, CQ) 


I)  =rRo]i.  j| 

COV  (N;  ' 
70  1=1, SS 
70  J=I,NS 
Jj  =C0  J  I,  J)  -B»  (I,  J)  -3A  (J,  I)  *GN  (I,  J) 

9780" 

SCOV     (NS, SC, « 11, C«R,CW I, NS,SC, a 11 ,CWa,CWI,CQ,GM) 
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(NC.EQ.O)     GO    TO    20  2 
DO    190    " 


9780    IF 

I    =  1,NS 

DO    19  0   J    =  1,  NC 

PRO  (I«J)     =  0.  DO 

DO    191    K    =  1. NS 


uu     if  i     n    =      i  •  na 
191     PRO  (I, J)     =     PRO(I,  J)  ♦GM(I,K)  CF53C  (J,K| 
190    CONTINUE 

DO    20  0    I    =    1,  NC 

DO    20  0    J    =     1.KC 

SC(I.J)     =    0.D0 

DO    201    K    =     1.NS 
20  1     SC(I.J)     =  SC  (I, J)  +  F3GC  (I,K(  *  PRO(K,J) 
200    CONTINUE 
202    IF(IREG    .EQ.    0)     GO    TO    9791 

DO    9792    1=1, NS 

DO    9792    J=1,NS 
9792    CQfl, J1=GB  (I.J) 

GOTO    503 
9791     WRITE(6,220) 
220    FORMATS  0'     //,2X     'THE    COVARIANCE    OF    THE     ESTI  »  ATE  .  .  '  ,  //) 

CALL    RAPRNT  (MH, BH.MH,  5.  GH,  »,'  (5(1X,1PD13.6))  ') 

IF(I?.GT.2)     GOTO    503 

DO    67    I    =    1,BH 

DO    67    J    =    1,BH 
67    CQ(I,J)     =    GN(I,  J)  *GM(I,J) 
503    CONTINUE 

WRITE  (6,210) 
210    FORBAT(' 0'  ,//,2X,  'THE    STATE    COVARIANCE     H ATR I  X. . «  , //) 
~rT(HH,HK,HH.5,CQ,  »  f  '  (5  ( IX,  1PD  1  3.  6)  )  ') 


CALL    RAPRNT  (BH.BK,  BH.  5,  CQ, 
IT     (NC.EQ.O)     GO    TO    232 
WHITE  (6,  221 
1     FORBAT(' 0* ,//,1X, 'THE    CON] 


221     FORMAT^1  0*  ,//,1X,  'THE    CONT R OL    COV ART  A N C E' ,//) 
DO    230    I    =     1,NC 

230  WRITE(6,231)      (SC  (I  ,  J)  , J= 1 , N  C) 

231  FOB  MAT  (  1P6D1U.6) 

232  DO    240    I    =     1,  NS 

2U0    CO(I.I)     =    DSQRT  (CO  (I.I)  ) 
IF    (NC.EQ.O)     GO    TO    25  1 
DO    250    I    =  1,NC 

250  SC(I.I)  =  DSQRT  (SC  (I,  I)) 

251  WRfTE  (6,262) 
262  FOP.  MAT  ('0'  .  iX,'  ST 

w 


:ATE    RBS    RESPONSE' ,23X,  'CONTROL    RBS    RESPONSE' , 


270    1=1 ,NS 


IF  (I.LE.NC)  WRITE  (6,272)  CQ  (] 
72    FORrtATC     '  ,  1PD15.  7.25X.D15.  7) 

IF  (I.GT.NC)  WRITE  (6,272)  CQ  (I  , 
70    CONTINUE 


CQ     (1,1)  ,SC(I,I) 
I) 


389    IF(ITF3    .PQ.    0)     GO    TO    KUO 
FORM    COnrENSATOR    FROB     BEAS    TO 


INPUT     AND    COBP'JTE 


DO    U10    1=1, NS 
DO    410    J=1 ,NS 
SUB =0. DO 
DO    U05    K  =  1,NO 
U05    SUB=5UB+FPGE  [I.  K)  »HO(K,  J) 
010    C0(I. J)  =ACL  (I, J)  "SUB 
WRITE  (6,  92u0) 
92"0    FORr.AT('  0'  ,//, 
ITPXO 
IZERO=0 

CALL    TP  (NS , NS,NSQ,CQ, AA, NO,  PBGE  ,  B'.  ,  NT  ,  ?  1GC  ,  C-.  ,  I  Z  S  RO    ' 
e  WFrWI,CWP,CWt,SC,JC?,PES,Cl,  02, DDD, EPS , ITP3, I TFX) 

UUO    CONTINUE 
C 

C    COBFUTE     PSD    FUNCTIONS    OF    THE    CONTROLLED    STSTEfl 
C 

IF(I?SD    -EQ.    0)     GO    TO    U50 
IF(irO    . LT.     3)     GO    TO    UU4 

CALL    PSDCAL(B,SS,Rfi,X,NC,3W  , GV . r S^C,  SO,  HT , H0 , HO, P PGE . NG  . 
1    GAR,  ACL,EA,W?,»I,d1.  D2,JCP,3ES.Q.RC,  36, CC,  1        ,  I  PSD,  t  NO?.  B) 


21, 'CCBPENSArOR    TRANSFER     FUNCTIONS...') 

IZERO, D, BB,CC, C ?, 
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nuu 
U50 


390 
395 


9771 
9765 


9763 


9768 
9764 


9766 
9767 


CALL 
1  GAM, 
GO  TO 
CALL 
1  GAM, 
IF(IS 
IF(NC 
DO  3  9 
DO  39 
ACL  (I 
CONTI 
CALL 
READ  ( 
WRI  TE 
FOR  MA 
FOSSA 
WRITE 
DO  97 
HI  (II 
DO    97 

WI(U 
DO    97 

CR(I) 

DO    97 

CP(I) 

WRITE 
DO    9  7 


PSDCAL  (M,NS,RM.X,NC,GW ,GV,  FBGC.NO,  HY  ,  HY  ,  HO,  FBGE  ,  NG, 
ACL.3A,WR,WI,D1, D2, JCF , RE5 , Q, RC, 3B,CC,2       ,IPSD,INORB) 
USO 


PSDCAL(M,NS,RM,X,NC,S» , GV, FBGC, NO.HY . HC, HO. FBGE , NG, 
ACL.BA.WR,  WI    Dl,  D2,JC?,RES,Q,RC,  BB,CC,ltOf  IPSO,  INOHH 
S    .  EQ.     0)     RETURN 


S    .  EQ.     0) 

NE.     0)     GO    TO    3  95 
0    1=1 , NS 
0    J  =  1  ,  NS 
,j[    =     5A  (I, J) 

MIN7(NSQ,ACL.NS,  DDD.D1  ,D2) 
5 


7U44)  (WR (I)   .1=1, NG) 

[6,9771)      (WR  (I) , 1=1, N3) 

Tf'O*,'     STEADY    DISTUR3AN... 

T (//////,'     STEADY    STATE    VALUES    OF    STATE    VAR. 
(6,  9765) 
63    1=1, NS 


CE    VECTOR '  ,/,  10  (1X,  1PD16.  6/)  ) 

ARE ■) 


=0.0 

63    J=1  ,NG 

=  WI  (I)  *GAM  (I,J)«- WR(J) 

6a    1=1, NS 

=0-0 

68    J=1 ,NS 


=  CR(I>  -ACL  (I,J)»  WI  (J) 
■    ,6000) 


DO 

CIiIl 

WRITE 

FORMA 
RETUR 
END 
SUBRO 
I  MP  LI 


-<^n  ill -«vl  IJ.  ««. 

(b,6000)     CR(I) 

66    1=1, NC 

=0.0 

66    J=1,NS 

=CI  (I)  +F3GC  JI,  J)  *CR  (J | 

(cijr 


(6,3767)      (CIjl) .I=1,NC) 

T(///,'     STEADY    STATE   CONTROL    IS    •.///,  1  0  (  1PD15  .  5/)  ) 

N 


UTINE    CDIV     (A,B,C,D,E,  F) 
CIT    REALMS     (A-H.O-Z) 

THIS    SUBROUTINE    COMPUTES    THE    COMPLEX    DIVISION 

E    ♦     ?«"!    =     (A    ♦     B»I)  /  (C   ♦     D»I) 


10 

100 
20 


10 


10 


1=^0 
E=(  A3 
F=iB5 

RETUR 
END 
SUBRO 
REAL* 

DIM  EN 
NU=L 

no  20 

IF 

DO 
WRITE 
WRITE 
FOR  MA 
NU-NU 
RETUR 
END 
SUBRO 
1W21  ,L 
IMF  Li 
DIM  EN 
D I M  E  V 
D I M  E  N 
K  »  1 
KP  » 
KN  = 
NRZEV 
NCPZE 

IP(«:. 


♦D'  D 

C*E"-D)  /T 
C-A^D)  /T 

N 


UTINE     FA  PR  NT (N MA  X.M.N,  L,  A,  I  DIM,  r MT) 
fl    A(NMAX.S) 
SION    PMT(IDIM) 


NL=1 ,N,L 
O.GT.N)     NU  =  N 

1  =  1, M 

fc,FMT)   (A  (I.J)  ,J=NL.NU) 
(6.10  0) 
T(»     ') 
♦  L 
H 

UTINE     RGAISfK, N? ,NC,N0B,WR, WI, VP, GN.W11  ,TCB, 

t,c,ci.ct,mh3, nx£ 

ClT    REAL»8     (A-H,0-Z) 

SION     WP  («)  ,  .1  (M)   ,VF  <M,  M)  ,SN  (NS,NS) 

SION       Wl  1  (NS,NS)  ,TCS(M  .  M)  .  W21  (  NS  .  NS)  ,  L7  (NS|  ,MT(N 

SION    C  (NS)  ,CI  (NS)  ,CT(NS,NS) 

1 
1 
=»    0 

v  =  o 

GT.M)     GO    TO    20  0 


S) 
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C   CHECK    FOR    EIGVAL    AT    OR    NEAP    J-OMEGA     AXIS    TO     INCLDDE    IN     E-L    EIGSTS 
C      TORN    FIRST    ONE    POSITIVE    AND    SECOND    ONE    NEGATIVE 


30 


35 


EI3  VR=DAES  (WR  (K)  ) 

IFfEIGVR    .GE.     1.D-10)     GO    TO     48 

IFjWI  (K)  )     40,30,40 

NRZEV    =     NRZEV*1 

IF(NRZEV    -GT.     1)     GO    TO    35 

WP.(K)     =    EIGVR 

GO    TO    75 

WKfKl     =    -EIGVR 

WRITE 


WRITE  (6,9000) 
9000    FORKAT(*0' ,'     EU LER- LA GRA US i 
»  '     OH     NEAR    ZERO.  ■  /) 

GO    TO     110 
40    NCPZEV    =    NCPZEVO 

IFfNCPZEV    .GT.     1)     GO    TO    45 

WR(K)     =     EIGVR 

WR(K*1)     =    EIGVR 

GO    TO    80 
45    WP(K)     =-EIGVR 

WRJK+1)      =    -EIGVR 


EQOATIONS     HAVE     A    REAL     EIGENVALUE     AT', 


90 


WFITE  f6,9010) 

10    FOS(1AT('0'  ,  '     EULER-LAGRANGE     EQUATIONS     HAVE     A    COHPL 
O          'EIGENVALUES     AT    OP     NEAR     THE    J-OME3A     AXIS.') 
GO    TO     120 
8    IF(WR  (K)  )     100,50.5" 
0    IF(WI  (kJi     80,75,60 
EIGENVECTOR 


EX     PAIR    OF     ' 


FOR    REAL 
GO    TO    78 


EIGENVALUE, POSITIVE    FEAL    PART 


EIGENVAL3E, POSITIVE 

75  IF(NOB.     EQ.    0) 
DO    76    J     =    1,3 

76  TCB  (J,KP)     =    VF(J,K) 
78    KP    =     KP+1 

K=K*1 
GO    TO    10 
C       EIGENVECTOR    FOR    COHP LEX 

80  IF(NOB    .EQ.    0)     GO    TO    83 
DO    81    J    =     1.M 
FE    =     VF  (J.K) 
FI    =    -VF(JfK*1) 
TCB  (J,KP)     =    FR*FI 

81  TC3(J,KP*1)     =    FR-FI 
83    KP    =     KP*2 

K    =     K*2 
GO    TO    10 
100    IF(KI  (K)  )      120.1 10, 120 

C       ; EIGENVECTOR     FO  I 

1  10    C(KN)     =     WR  fK) 
CI(KN1     =    Wf  (K) 
IF     (NOE.NE.     0)     GO    TO    96 
KNS     =    KN*NS 
DO    95    J=    1,H 
95    TCB  (J, KNS)     =    VF  (J,K) 
9  6    KN    =     KN»1 
K  =  K*1 
GO    TO     10 

EIGENVECTOR    FOR    COMPLEX    EIGEN  V  ALUE,  N  EG  ATI  V  E    REAL    PART 


)R    REAL     EIGENVALUE,  NEGATIVE    REAL     PART 


120    PR 


R  =  WR(K) 
I  =  WI(Ki 
(KN)      =     r.R 


C(KN*1)      =     S3 

CI(KN)     =    RI 

CI(rN*1)     =     -    RI 

IP     (NOB.SE.     0)     GO    TO 

KNS     =     KN*NS 

DO     121    J    =     1,B 

FF    =     VF  (J.r.) 

FI    =     -VF  (J,K*  1) 

TCB  (J, KNS)      =    FB*FI 

121  TCB  (J.KNSO)     *    PE-FI 

122  KS    =    KN»2 


122 
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K  =  K+2 

GO  TO  10 
200  CONTINUE 

IF  (NOB. NE.  0)  GO  TO  321 
:   PORMATION  OF  M11 

DO  300  I  =  1,NS 

DO  300  J  =  1. NS 

W11   fl-JJ     a    TC3(I,J+NS) 
300    CTfl,  J)     =    H1 1  (1,5) 
:       FORMATION    OF    H21 

DO    320    1=1, NS 

DO    320    J=1 , NS 

320  W21(I.J)  =    TCB  (I+NS,  J*  NS1 

321  IF     (NOB.    EQ.     0>     GO    TO    323 
DO    32  2    I    =     1,N5 

DO    32  2    J    =     1.NS 

W21  (I, J)     =    -TCB  (I.J) 

322  Wlljl.jj     =    TCB(I+NS,J) 

323  CONTINUE 
:      INVERT    W 11 

NS0  =  N  S*1  NS 

CALL    HINTfNSQ.H11.NS. DETC,LT,Hr) 
:      CALCULATE   THE    RGAIN    HATBIX 

DO    325    IL=1,NS 

DO    325    JL=1 ,NS 

GN(IL.JL)     =    0.D0 

DO    325    KL=1,NS 
3  25    GN(IL.JL)=GNfIL. JL) +W21 (IL,  KL) * H 1 1 ( KL , JL) 

IF     (NOB.EQ.     0)      RETURN 

DO    5999    I    =    1,NS 

DO    5999    J    =    1,NS 
5999    CT(I.J)     =    W11  (J, I) 

RETURN 

END 

SUBROUTINE     MI  NV  (NSQ  ,  A  .  N.  D,  L  ,  M) 

IMPLICIT    REAL*8     (A-H,0-Z) 

DIMENSION  A  (NSQ)  ,L  (HI  ,H  (  N) 

DOUELE  PRFCISION  A,  D,  31 J  A,  H  OLD 
NM=N*N 

D=1 .0D0 

NK=-N 

DO  £0  K=1,N 

NK=KK*N 

L<K  =  K 

n  (K  =k 

KK=NK*K 

t-IG  A=  A(KK) 

DO  20  J  =  K,  N 

IZ«K*  (J-1) 

DO  20  I  =  K,N 

IJ=IZ*I 
10  IFf  DABS(BIGA)-  DABS(A(IJ)|)  15,20,23 
15  bi:}j  =  A(IJ) 


y«i:J 

CCNTINU! 


20  CC 
C 

C  INTERCHANGE  ROWS 

C 

J  =  L  (K) 

IF(J-k)  35,35,25 
25  KI=K-N 

DO  3  0  1=1, N 

KI=KI*N 

HOLt=-A (KI) 

JI=  KI-K*J 


A  (KI)  =»  (JI) 
30  A(JI)   -HOLD 

INT 

35  I=M  (K) 


C 

C  INTERCHANGE  COLUMNS 

C 
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38 


40    A 


m 


45 
46 

48 

50 

55 


IF(I-K)     45,45,38 
JP=NI>  (1-1) 
DO    40    J=1,N 
JK=NK*J 
JI=JP*J 
HOLD=-A (JK) 
=  A    Jli 
=  tiOLD 

DIVIDE    COLUMN    BY    MINOS    PIVOT     (VALOE    OF    PIVOT    ELEMENT    IS 
CONTAINED    IN    BIGA) 

GA)     48,46,48 

DO 

N 

1  =  1.  N 
K)     50,55,50 

=  AfIK)/(-BIGA) 

NUE 


IFfBI 

D=d.0 
RETUH 
DO  55 
IF  (I- 
IK=NK 
A  (IK) 
CONTI 


FEDUCE    MATRIX 


60 
62 


6  5    CC 


DO    6  5 
IK=NK 

hol:  = 
ij=i- 

DO    65 

IJ=IJ 

IF(I- 
IF{J- 
KJ=IJ 
A  (I  J) 
HTI 

DI 


70 
75 


80 

100 

105 
108 


1  10 
120 

125 


KJ=K- 
DO  75 
KJ=KJ 
IP(J- 
A<I?J) 
CONTI 

IF 

D  =  D'  B 

FF 


1=1, N 

♦  I 

A  (IK) 
N 

J  =  1,N 

♦  N 

K)     60,65,60 

Kj     62,65,62 

-I*K 

=  HCLD<»A(KJ)  «-A(IJ) 

NUE 

VIDE    ROW    BT    PIVOT 

N 
J=1,  N 

♦  N 

K)  70,75.7  0 
=A(KJ) /BIGA 
NUE 

ODUCT    OF    PIVOTS 

IGA 

PLACE     PIVOT    BY    RECIPROCAL 

=  (1.000)  /BIGA 
NUE 

NAL    ROM    AND    COLUMN    INTERCHANGE 


K  =  N 

K=fK- 

IP(K) 

i  =  l  (k 
iFjr- 

JO=  N>5 

J?.=  N<* 

DO    11 

JK=  JQ 
HOLD  = 
JI  =  JR 

im 

J  =  M  (K 
IP(J- 

Ki=r.- 

DO     13 


1) 
150,  150,105 

K)      120,120,108 

t\\ 

6    J=1 ,N 

♦J 

A  (JK) 

=  -AMI) 

=  HdLD 

k)      100,100,125 
N 

o  1=1, s 
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130 
150 


100 


50 


51 
52 

13 
10 
14 
20 


21 


KI= 
HOL 
JI  = 
A  (K 
A  (J 
GO 
K  =  0 
RET 
END 
SUB 
BEA 
t 
PEA 
DO 
DO 
X  (I 
DO 
X(I 
DO 
DO 

Q(l 

COM 

1=1 

IF 
J  =  1 

IU 

B=- 
C  =  A 
D=C 
K1  = 
K2  = 
K3  = 
KU  = 
11  = 
J1  = 
X  (I 
X  (I 
x  (I 
X  (I 

J=J 

GO 
A=V 
E  =  A 
K1  = 
K2  = 
X 
X 

J=J 


KI+H 

D=A(KI) 

KI-Kt-J 

r 


>1  -RTO 

:)  =-A(Ji) 

:       =HOLD 

-6   100 


URN 


ROUTINE    SCOV     (ML,  H  L  ,  WLI  ,  VL  1-  VL2,  NR  .  KB  ,  WPI ,  V  R1 ,  VP2  ,  Q,  X 

7L1  (NL)  ,  VL2  (NL)  ,8L(N  L,N'L)  ,  WLl  (NI 
VR1  (NR)  ,  VR2(NR)  ,  Wfi  (NR,NR)  ,  WBI  (WR,NR) 
L*8    i,3,C    n    " 
50    1=1, NL 


NR)  , 


50    J=1, NB 
.J)=0. 

50  11=1 , NL 

,J)=X  (I,  J)  +WLI(I,II)  *Q(II,J| 
52    1=1, NL 
52    J=1, NR 
,J)=0. 

51  JJ=1,NR 

,  J)  =Q  (I,  J)  *X(I,  J  J)  ""WRI  (J,  J  J) 
TINUE 

(VL2(I))      10,11,10 

VR2(J))     20.21,20 
LI  (I)  +VR1  (J) 
2.*VL2(I)  *-VR2  (J) 
i»2*VL2  (I)  »•>  2*VR2(J)  f  32 

A^C/D 


-  (VR2  (J)  -OVL2 

-  (VR2  mrB  +  VL2 
-A^B/D 


2  (I)»B)/D 
IJ'CJ/D 


1*1 

J*  1 
,J) 

a 

1.J1)=»KUOQ(l,J)-K3*Q(:,J1 
♦  2 


=  *K1*Q(t,J)*K2*Q(T,J1)*K3'0(rifJ)*KU^0(ri,J1] 
=  -K2>Q(I,  J    ♦  K1*Q{I,Jl}-KU'»Q{I1,J)  ♦K3'0(I1,J1 
*-K3*Q<I,J)  -Ka»Q  (I,  J1)*K1»Q(T1,J)  ♦K2f:Q(ri,J1 

K2-Q(r  i,  j)  *k  i'  j(r  i  ,ji 


R1  (J)  *VL1  (11 
ot  2*VL2  (I)  "*2 


\l 


Hi 


fc/B 

VL2(I)/B 


,J)=K1>Q(I,J)  -K2*0  (1*1,  J) 
♦1  ,J)  =K2HQ  (I, J)  ♦K1"Q(I»I  ,J) 

NR)     GO    TO    1U 


1  1 

15 
30 


31 

32 
12 


♦  1 
J.  LE. 

♦  2 
TO    12 


(VP2(J    1     30. 
r  K 1  (J)  ♦  VL1  (if 

l!"k/»VR2  (J)  "t" 
A/B 
VR2(J) /B 


GO 

J  =  1 

IF     (VP2(J) I     30, 31,30 

B«=A*"*2*VR2  (J)'f,h: 
K1=, 

K2=< 

X(I,J)=Kl»0(I,J)-K2'kg(I,J*1) 

X  (I  ,J*1)  =  K2'2(I,J)»Kl'JQ(I,J*1) 

J=J-   " 

GO 

X  (I 

J=J 

IF 

1=1' 

IF 

DO 

DO 

Q(I 


♦2 

TO    3  2 

,J)=>Q(I,J)/(V?1  (J)  «-VL1(I)) 

(J.LE.NS)     GO    TO     15 
♦1 
(I.  LE.SL1     GO    TO     13 

uo  1  =  1,  nL 
uo  j«i, sa 
,J)    =o. 
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DO    UO    11=1 , NL 
«0    Q(I.  J)     =    Q  (I, J) 
DO    U2     1=1, NL 
DO    H2    J=1,NE 
X  (I,J)=0. 
DO    41    JJ=1,NH 

41  X  fI,J]=X  (I, J)  *Q(I,JJ) 

42  CONTINUE 
RETURN 
END 
SUBROUTINE 


+     WL(I,II)  >  X(II,J) 


-WR  (J,  JJ) 


10RM 


MODE(WNOSH,G,GH3  RM,NS,N1  ,H2,  XC?V) 
MATRIX    U     OB    U-INV 


TRANSFORMATION 

NO.  OF  STATE 

NO.       OF    INPUTS    OS    OUTPUTS 


UNI 

NS 

NC 

ICON       CONTROL    FLAG    TO    INDICATE     WHICH    TR A NS PO? • ATION 


MODAL    G 

MODAL    GAMMA 

MODAL    H 

MODAL    C 

MODAL    K 

CONTROL    EIGENVECTD 

MEASUREMENT     EIGEHV 


A-H.O-Z) 
NS,NS).G fNI 

, • MODAL  CON 
X, 'MODAL  PR3 
X,' MODAL  MSA 
•THE  MODAL  C 
CONTROL  EIG 
MEASUREMENT 
1P6D14 .6  ) 
AL  FILTER  ST 


R  MATRIX 
ECTOR  MATRIX 


,N2)  ,GNORM  (SI  ,  S2j 

TROL    DISTRIBUTION    MATRIX..'     //) 
CESS     NOISE     DISTTI-UTION    MATRIX       •     //I 
SURSMENT    SCALIK.-I    rATRIX'     //)  '        ' 

ONTROL    GAINS     APE-' //)        '        ' 
ENVECTOR    MATRIX.     • ' //{ 
EIGENVECTOR     MATRIX..1 ,//) 

EADY    STATE    GAIN? '  ,//) 


0,2  50, 75,250,250)  , I  POINT 


MjT, J)  +WNORM 
250,250,160) 

NORM  (I  ,J)  ,J» 


(I.K)fG(K,  J) 

,irOINT 

1,N2) 


,  NS 
,N1 
NS 
'=    GNORH  (I, J)      ♦     G 
.261, 261  ,262,261, 
70) 


JI.K)  "WNOPM  (K,.l) 
263,  264)  ,IPdlNT 


WZ(D 


100)  (GNORT.  (I  ,J|  ,J»  1,RS) 

NS,IWPITE,N3Q,DnD,D1,D2,WNORM, WNOS  MI. 
PEAL  PART  OP  I-TH  EIGENVALUE 


CNO?.M(WZ,tf  t.ve: 
N1,N2) 
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aY(i) 

VEC 
NS 

IWRITE 
WNORH 


COMPLEX    PART     OF    I-TH    EIGENVALUE 

MATRIX    OF    RIGHT    EIGENVECTORS     STORED    IN     REAL    POSH 

FROM    HQR2 
NO.     OF    STATES 

FLAG    TO    CONTROL    FORMATS     FOR    DIFFERENT    EIGHENSYS TSMS 


NORMALIZED    MA 

BY    COLUMNS    I 

WNORMI  0-INVERSE    2>C 

STORED    BY    P 

NSQ,DDD,D1, D2    -    ARGUMEN 


TRIX    U    OF    RIGHT    EIGENVECTORS    STORED 

N    SEAL    FORM 

CNGUGATE  DP  LEFT  EIGENVECTORS 

OW  IN  REAL  FORM 

TS    PASSED    TO    MINV 


IMP 
REA 
DIM 


9030 
90UO 
9050 
Q060 
9070 
9080 
9090 
9100 
91  10 
9120 
9130 


DAT 
I 

FOR 
FOR 
FOR 
FOR 
FOR 
FOR 
FOR 
FCR 
FOR 
FCR 
FOR 


ALC8  (A- 
D,  CO  KM  A, 
Z(NS) , WY 
(NS.NS)  , 

2.5/ 

,BIGHT/1H1  /,  FMT/ 

'OPEN    LO 


LICIT  RE 
L^8  FIEL 
ENSION  W 
,  WNORMI 
CM(N1,  N 
A     FIELD/ 


HtO-Zl 
SEHCOL.  R 
(HS^,VE 


)HE1 


IGHT, FMT 

(NS.NS)  ,WNORN  (NS,NS) 
TORE  (6)   ,D1(NS),D2(NS),FMT(1U)  ,  HO  (N1,N2)  , 


1  1    FOE 


MAT 
MAT 
MAT 

MAT 
MAT 
MAT 
MAT 
MAT 
MAT 
MAT 
MAT 


0" 
■0', 
•0«  , 
'0'  , 

//'6 
//•o 
//•o 
//•o 
//•o 
//•o 


CLOSED 
CLOSED 
CLOSED 
CLOSED 
,  'RIGHT 
,  'OPEN 
,  'CLOSE 
,  •  C  LO  S  E 
,  'CLOSE 
, 'CLOSE 


.COMMA/5 
6H  (IX, IP 

OP  EIGEN 
LOOP  OPT 
LOG?  SUB 
LOOP  OPT 
LOOP  SUB 
EIGENVE 
LOOP  LEF 
D  LOOP  0 
D  LOOP  S 
D  LOOP  D 
D  LOOP  S 


VALU 

IMAL 

OPTI 

IMAL 

OPTI 

CTOR 

T  EI 

PT. 

UBO? 

PT 

OBOP 


',  / 
1H  / 
ES-. 

REG 
MAL 

EST 
MAL 

MAT 
GENV 
REG. 
T.  R 
FILT 
T.  F 


EMCOL/5H,': • ,/ 
EMEND/UH, ': '/ 

ATOR  EIGENVALUES..') 
GULATOR  EIGENVALUES..') 
ATOR  EIGENVALUES..') 
TIMATOR  EIGENVALUES..') 
X.. '//) 

TOR  MATRIX.  .  •) 
EFT  EIGENVECTOR  MATRIX..') 
.  LEFT  EIGENVECTOR  MATRIX..' 
LEFT  EIGENVECTOR  MATRIX. .i') 
TER  LEFT  EIGENVECTOR  MATRIX. 


MAT  (U6X 

NORMALIZE    CCMPL 


,'  (•  ,F10.7.«)  *J(  «,F10.7,  «|   ') 

EX    EIGENVECTORS     BY    LARGEST     ELEMENT 


990 
997 


980 


991 
999 


KK  = 

LR= 
LC= 

rn 

IF{ 

IF 

LC  = 

FMA 

DO 

CMO 

IF 

EMA 

M  =  I 

CON 

VMR 

VMI 

DO 

VR 

VI_ 

VFC 
ilHD 
WK3 
CON 

nr* 

GOT 

kf* 

CON 


0 

0 
0 
J99 


"(WY 


(DAES 

LC*1 

X     =    0.D0 

997    I    = 

D=VEC  (I  . 

(CMOD-EM 
X     =    CMOD 

TINOE 
=  VEC(M 
=  VFCfM 
980  1=1, 
=  VEC  (I, 
*  VECfr. 
=N=(VK» V 
IN*  ]-VR«' 
3(1  (I,  K)  = 

sm  (i.r*  i 

TINUE 

1 

0 

0 

TINOE 


,NS 
GOTO    991 
(K))  .LT.  1.  D-10) 


GO    TO    999 


1,NS 

K    »  s*2*VFC  (I.  K+1 

AX) 997,990,990 


)f»2 


,K) 

NS 
R) 
K*1) 

MR*VI»VMI) /EMAX 
VMI* VI'VMR)/2MAX 
VECRN 
) =VECIN 


999 


MGP.HILIZE    REAL     EIGENVECTORS     BY     THE   TOTAL    LENGTH 

,NS 

K)  )  .GE- 1.D-10)     GOTO     1000 


DO 

in 


1000    f=1 
DABS  (WY  (I 
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998 
996 


995 
1000 


520 

530 

540 

545 

550 
560 


LE=LR*1 

RESOD    =    O.DO 

DO    996    1=1, NS 

RE!10D  =  VEC(If  K    >  **2«-REflOD 

RP3D=DSQRT  (REHOD) 

DO    995    1=1  ,  NS 

RVEC=VSC  (I,  K)  /R.IOD 

WNORH  (I,  K)  =  BVEC 

CONTIN0E 

CONTINUE 

GO    TO     (520.530,540,545,550)   ,IWRITE 
WRITE  (o,  9030) 
GO    TO    5b0 
WRITE  (6,904  0) 
GO    TO    560 


WRITE  (6.  O050) 

GO    TO    560 

WRITE  (6,9060) 

GO    TO    560 

WRITE  (6,9070) 

KK=0 

NPHTV=0 

NFMTV=1 

DO    568    1=1, NS 

IF(KK    .  EQ.      1)     GO    T 

IffbASS  (WY  (I)  )     .GT 


GO    TO    567 

1.D-10)      KK=1 


C 

C    PRINT    OUT    KC    MORE    THAN    6     WORDS,     NOT    SEPARATING    COMPLEX     EIGVAL 

C 


5    .OP, 


rw   .  LT 

1TW4-1)     =    RIGHT 

>,F!1T} 


(NPRTW 
(STORE  (J)  ,J  =  1 


•  EQ.     5    .AND. 
NPRrW) 


KK    . EQ.     0) )     GO    TO    561 


561 


562 


567 
568 


570 

575 
578 
580 

590 

600 

610 


JPFTWO 

;  f  m  w  ♦  i 

,  EC-      1)     GO    TO    562 

iPRTW)     =    WZ(I) 

1TW)      =    FIELD 

=     NFt1TW*1 

•TW)      =    SEflCOL 

>68 

=    WZ(I) 
FIELD 
=    COflflA 

;  I  KT  W ♦ 1 )     =    WYJI) 

1TW*2)     =    PIELD 

1TW»3)     =    SEHCOL 

'     NFPITW    ♦     3 

«     NPRTW    ♦     1 

)68 


IPRTW) 
1TW)  = 
TW*  1) 


:hend 

=    RIGHT 
(STORE  (J)  ,J  =  1,  NPHTW) 


IS,NS,NS,6,WNOHf1,4,  '  (6  {  U  .  1  PD 1 3  .  6)  )  •) 
IS,HS,NS.6,WNDRB.4    '  ?6     1PG12.6)  )  ' 
'0,  570,  575.575)   ,lWRlfE 
)KM,HO,C.-.,NS,N1  .  S2,  5) 


>R!«,HO,Cr.,. NS.S1  ,  N2,  6) 
>  90,600,  610,  o20)  ,IWRl 


TE 


(6/ 
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GO    TO    6  30 
620    BRITE  (6,9130) 
C    SAVE    D-INV    OPEN    LOOP    IN    WNORBI 
o30    IF(IWRITE    -GT.     1)     GO    TO    103  5 

DO  510  1=1, NS 

DO  510  J=1 , NS 
510  BKOEBIfl. J)  =WNORM  (I.J) 

CALL    HINV(NSQ,*NOHBI, NS.DDD.D1,  D2) 

CALL    RAPRNT  (NS, NS,NS, 6,WN0R MI, ft,  '  (6 ( 1 X,  1 PD 1 3. 6) )  •) 

RETURN 
1005    CALL    MINV(NSQ,WNORB ,NS,DDD.  D1 ,D2) 

CALL    RAPRNT  (NS,  NS,NS,  6,WNORB,4,  •  (6(1X,1PD13.6))') 

RETURN 

END 

SOB  ROUTINE    TF (N , N M , NS Q, A , A  A , H ,B  ,  BM,  L,  C,  CM , I FPFW, D , 3B , CC,  CP, 

*  EVP..EVI.P8.  PI,SC,  JCF,3ES,D1,  D2,DDD,  EPS,  ITF,ITFX) 
ir?LICIT    REALMS  (A-H,0-Z) 
DIH  FN  SI  ON    A  (N.N)  ,  AA  (N  ,N    ,3  (  N,  M)  ,  BM(N.B)   ,C  (L,N)  ,CB  (L,  N)  ,D(L,M)  , 

*  BB(N)  ,CC(N)  -CP  (N)  .EVfitfJ)  ,  EVI  (N)  ,?R  (N)  ,  PI  (N)  ,  SC  (N  ,  N)  ,JCF(N) , 

*  RES  (N>  ,D1  (N)  -D2  (N) 
C       SAVE   COBPUTATIOON    ON    OL     AND    CL     SYS    WITH    MODAL    WORK    DONE    IN    CPTSYS 

IF(ITFX     .EQ.     1)      GO    TO    50 

IFjITFX     .EQ.     2)     GO    TO    5 

CALL     POLES (N, NM,A,AA,M,B,L.C,?R.PI,D1,D2,JCF,SC| 
C      COMPUTE     MODAL    MATRICES    FOR    RESIDUES 
5    DO     10     1=1,  N 

DO     10    J=1,N 
10    AA(I.J)      =    SC(I,J) 
15    DO    2  0    1=1, L 

DO    20    J=1,N 

CS(I.J)     =    0.D0 

DO    20    K=1,N 
20    CR(I.J)     =    CB(I.J)     ♦    C  (I.K)  >  AA  (K,  J) 

CALL     BINV(NSQ,A  A,N,DDD,D1,D  2) 

DO    30    1=1, N 

DO    30    J=1,B 

5(1(1,  J)  =    0.  DO 

DO    30    K=1,N 

!C  (I.  J)     ="«  " 
5  0    CONTINUE 

DC     10  0    I  =  1  ,  fl 

DC     100    J=1,L 

IF(ITF    . NE.     3) 
"CALL     ZEROS ( I, J, IFDFW,  N, NM, A  , A  A, M, 3,  L,  C,  D , 3B, CC.  CT  , EV P , EVI , D1 ,  D2, 

*  EPS) 

IF(1TF    .NE.     2)     CALL    R ESI D (I  , J , N , JCF,  B  ,  3 B, L , CM , PR , PI  ,  R  ES  ,  BB  ,CC  ,  1) 
100    COMINUE 
R  FT  U  R  N 
END 

SUBPOUTINE    CHECK  (EPS,  NC,  KZ ,  NO) 
DOUBLE    PRECISION    EPS 
COMMON    /FROG/    IOL.  IKQ.IQ.IB ,ISS. IB.irFI , ITF2 , ITF3 , IFDFW, IE, IDSTAfl 

*  I  DEBUG,  IS  ET,  IP  EG,  IPS  D,  I  Y  U.I  NORM 

C       SET    MODAL    ANALYSIS     WHEN    OL    ElSENSYS    OR    DL    TF    REQUESTED 

IFlIB    .EC.     1     .AND.     IOL    .  EQ.     0)      IOL=1 

IF(IOL    .  FO.     3    .OR.     ITF1    .SE.    0)     XB*1 
C       CHECK    TO    SEE    IP    H    MATPIX     INPUT 

IF(NO    -NE.     0     .OR.     IOL    .GE.     2)     GO    TO    25 

■  RITE  (6,3000) 
3000    FORMAT^//'     H    -    MATRIX     MUST     BE    INPUT,     I.E.     NOB    .GT.     0«) 

STOP 
25    CONTINUE 
C 

C       TRANSFER     FUNCTION    CHECKS 
C 


30    SB  (I,  J)     =o«(I,J)     ♦     AA  (I,K)  *  B(K,  J) 

-ONTINUr 


IFIIE    .  EQ.     0>     IE  =  6 
Zti-10.->*  (-IE) 
:N    LOOP    TP 
IP(ITF1     -EQ.     0    .OR 
WRITE  (6,9000) 
9000    POSBAT(//'        INPUT  (G)      MATRIX     MUST     £ 
'  •  TO    COSPUTE    OPES    LOOP    T  .     F.      •) 


OPEN    LOOP    TP 

IFjITFI     -.EQ.    0    .OR.     NC    .  SE    .    0)     GO    TD    S  0 


EE    8  EQUESTEDd.  B.     NC    .  NE.     0) 
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STOP 
C      COMPENSATOR    TF 

50    IF(ITF3    .EQ.    0)     GO    TO    100 

IF(IREG     .EQ.    0    .AND-      (NC    .  N  E.    0     .AND.     S'S    .  NE.     0|  )      GO    TO    100 
WRITE  (6  ,  9100) 
9100    FORMAT(//'        REGULATOR    AND    FILTER    SYNTHESIS    MOST    3E    REQUESTED     IN 
*  ,'THE    SAME    RUN    TO    COMPUTE    COMPENSATOR    T.     F. • ) 

STOP 
100    CONTINUE 
C    NOISE    TF 

IFJITF2    .EQ.    0)     GO    TO     150 
IF|nG.NE.    0    .AND.     NC    .NE.    3)     GO    TO    150 
WRITE  {6,  9200) 
9200    FORMAT*//'     NOISE    T.     F.    CALCULATED    ONLY     WHEN    REGULATOR    DESIGNED    ■ 
C  ,*AND    GAMMA    INPUT,    I.     S.     NG    . NE.     0') 

STOP 
C 

C       DESTABILIZATION    RESTRICTIONS 
150    IFIIDSTAB    .EQ.    0)     GO    TO    200 
IF    NC    . EQ.     0)     GO    TO    2  00 
IF(NG    .NE.     0       IPEG=1 
WRITE  fb,9300f 
9300    FORMATf//'     DESTABILIZATION     OPTION    DESIGNED    FOR    A    REGULATOR    OR     ', 


WRITE  (b,9300i 
FORMATf//'     DE! 
*  'FILTER    BUT    NOT    EOTH    SI  MULT  AN EOUS LY • ) 


IFflREG    -EQ.     1)     GO    TO    200 

STOP 
200    CONTINUE 
C 

C    PSD    INPUT 
C 

IFflPSD    .EQ.    0)     GO    TO    300 

IFflPSD    .If.    0    .OR.     IFSD    .ST.    3)     GO    TO    250 

IFJIYU    .  LT.    0    .08.     IYU    .GT.     2)     GO    TO    250 

IF(INOPM    .LT.    0    .OR.     XNORfl     .GT.     NG+NO)     GO    TO    250 

GO    TO    275 


250    WRITE  (6,9400) 
9400    FOKHAT(l    **«■*«**• 


»"•    INCONSISTENT     PSD    INPUT    FLAGS    «" oi***+ 1 ) 
STOP 


275    IF(IREG    .EQ.    0    .AND.     NC    .NE.    0)     GO    TO    300 
WRITE  (b,  9500) 
9500    FORMATS     >*vfa«-*»BOTH    A    REGULATOR    AND    FILT2P    MUST     BE    RESIDENT     ', 
1     'TO    COMPUTE    THE    PSD    OF    A    CONTROLLED    SYSTEM!') 
STOP 
300    CONTINUE 
n  £T  U  R  N 
END 

SUBROUTINE    FOLES  (N,  NM  ,  A,  A  A,  H,  B,  L,  C,  EV  3  ,  EV  I,  D1  ,  D2  ,  JCF  ,  SC) 
IMPLICIT    REAL<*8  (A-H,0-Z) 

DIMENSION    A  fN,N)  ,  A  A  (N.N)  ,B(N,M)  ,  C(L,N)  ,  EVR  (N)  ,  EV  I  (N)  ,D1  (N)  ,02  (N)  , 
«*  JCF  (N)  ,SC  (N,N) 

DO     1     1=1,  N 
DO     1     J=1,N 

1  AA(I.  J)  =  A  (I.J) 

Qfl  *e-*n><~4  dJiftlirr:tC*-3tt»9 

CALL     EALASC  (NM,  N,  A  A  .LOW,  I  HI  GH,D1) 

CALL    OF  7 HE S  (NM, N.LOW, I  HIGH,  AA,D2) 

CALL    CRT  FUN  (NM, K. LOW.  IHIGH,  AA,D2,SC) 

CALL    HCR2(NM,N,LCW, IHIGH.AA  , E Vg , fe VI,  3 C, I  ERR) 

IF(IEPR     .NE.     0)     GO    TO     110 

CALL     PAL EAK(NH,N,  LOW,  IHIGH,  D1,N,SC) 

WHITE  (6,  101) 

101  FCRMAT(///,29H    TF    DENOMINATOR    EIGENVALUES:) 
DO    2     1=1. N 

2  WRITE  ffc,  102)     EVR(I)  .-VI  (I) 

102  FORMAT(/,2X,3H       (  ,  F  13  .  b,  uH|   ♦  J  (,  F  1  3.  6  ,  1  H)  ) 
RETURN 

1  10    WPITE  (6,9000) 
9000    FORMAT^     FAILURE    IN    HCR2,     CALCULATING     POLES') 
100     RETURN 
END 
SUBROUTINE     ZEBOS  (K1  ,  T.  2,  I  FDP  W,  S,  NM,  A,  A  A  ,  M  ,  B,  L,  C,  D  ,  B5,  CC,  C?  ,  EV?  ,  EVI 
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D1,D2,  EPS) 
IMPLICIT    REAL&8  (A-H,0-Z) 
DIMENSION    A  (N.N)  ,AA  (N,N|  ,B(N,!1)   ,C  (L,N)  ,  D (L,  M)  ,  BB  (N)  ,CC  (N)  ,  C?(N)  , 

EVR  (N)  ,  EVI(N)  .  D1  (N)  ,  D2  (  N) 

nnit  qt  r1     rD^r-TCTnu      c  -  t      nine 


EVR  (N)  ,EVI(N)  -D1  (N)  ,  D 
DOUBLE    FRECISION    SCL,DABS 
DO     1     1=1,  N 
EB(I)        =    B  (I,K1) 
CC(I)     =    C(K2,I) 
DO    1     J=1,N 


101 


uu     i     u  —  i  ,  a 
1     AAfl.J) =A(I,J) 

WHITE  (6,  10  1)     K1.K2 
FOBMATf///, 17K    TF 


NO.  ,I3,15B    AND    OOTPUT    N0.,I3,':,| 


.     FOR    INPUT 
IFflPDFW    . EQ.    0)     GO    TO    2 
H=    D(K2,K1) 

IF(DABS  (H)  .LE.EPS)     GOTO    2 
JJ=N 
GO    TO    5 

2  NN=N-1 
DO    3     1=1, NN 
H=SCL(N,  BE, CC) 
CALL    CCOMP (N.NM. AA,CC,CP) 
IF(DAES  (H)  .GT.EPS)     GO    TO    l» 

3  CONTINUE 
H  =  SCL  (N, BB.CC) 
WRITE  (6,  102)     H 

102  FCRHATf//, 5X,26HN 
GO    TO    100 

U    JJ=N-I 

5    WRITE  {6,  103)     JJ.H 

103  PORflAT(/,3X,20HORDER    OF    NUtl  ERATOR    =,I3,9X,8KTF    SAIN=,F13.6) 
CALL    ACOMP (N, NH,AA, SB,CC,H) 

Qr  axfirtiiiiui.  unLJii^niJvivftifi 


10    FINITE    ZEROS.        TF    GAIN=,F13.b) 


?R) 


CALL    BALANC  (NK,  N,  A  A  ,  LOW,  IHI  GH  ,D  1) 
CALL    ORTHES  (NM, N,LOW,  IHIGH,  AA.D2) 
CALL    HOR  (NN,N .LOW. I  HIGH-  AA,  EVR, EVI,IER1 
IF(IERR     .NE.     0)     GO    TO     110 

WRIT?  (b,  10U) 

104  FORMAT(/,3X,57HNUMERATOP     EIGENVALUES     (INCLUDING    EXTRANEOUS    ZERO    V 
1LUES)  :) 

DO    6     1*1. H 
6     WPITE(6,10S)     EVRfl)  ,  E  VI  (I) 

105  FOHr.AT(/,uX  ,'  (,,F13.6,,)♦J(',F13.6,,),) 
100     RETURN 

110    WRITE  (6,9000) 
9000    FO°MAT(*     FAILURE    IN    H  QR    CALCULATING    TRANSPER     FUNCTION    ZEROES') 
RETURN 


END 


1     A 


SU3POUTINF     ACOMP(N,NM,A,B,C,H) 

REAL"fl    A.b.C.H 

DIMENSION    A  (KB,  N)  ,  B  (HJ  ,C  (N| 

DO    1     1=1, N 

DO     1    J=1,N 


jIrjT  =  A  (I,  J)  -b(I)  >C(J)/H 


END 

SUBROUTINE    CCOMP  (N,  Nf!  ,  A,C,CC) 

PFAL'-S    A,C,CC 

DIMENSION    A  (NM,N)  ,C  (N)  ,  JC(N) 

DO     1     1=1, N 

CC(I)  =0. 

DO    1     J=1,N 

1  CC(I)  =CC  (I)  *C  (J)  "A  (J,  I) 
DO    2     1=1, N 

2  C(I)  =CC(t) 
R  ET  U  R  N 
END 

FUNCTION    SCL(N,  B,C) 

RFAL^e    B.C.SCL 

DIMENSION    B(N),C(N) 

SCL=0. 

DO     1     1=1, S 
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1     SCL=SCLfC(I)  ~B(I) 

RETURN 
END 

SU3PO0TINE     RESIDfKI^.N.JCP.  .1,  SB  ,  L,  CM  ,  PR,  PI ,  F.ES  ,  BB,  CC,  IPT) 
IMPLICIT    REAL^8  (A-H,0-Z) 

DIMENSION    JCF  (N)  ,  BB  (N,B)  ,  CB  (L,  H )  ,  PR  (N  )  ,  PI  (N)  ,  BES  ( N)  ,  BB  (N)  ,CC(N)  , 
C       PPT  (4) 
DATA     SN"/8H=»SIN(BST/,R  1/8H  =»/    R2/9HEXP  (A-T)  / 

DATA    ZERO/O.  DO/,  T  1/UH«'-T»*/,  BLANK/8H  /, CS/8 H*COS (B^T/ 

DATA     ED/1H)/ 
:       TEBPORARY     BOD    TILL    JCF    IS    CALCULATED 
DO    5     1=1, N 
5    JCP  (I)=0 
:       TEBPORARY    BOD 

IFflPT    .EQ.     1)     WRITE(6,9033) 
9000    FORMAT(//, 3X,*RESID0ES    AT    THE    POLES  :  '  /,  T 1 6  - • P    OLE    S',T41, 
C  >  R    E    S     I    D    0    E    S'  ,/,T9,  'REAL  (A)  ' , T26, ' I  BAG  (3)  ') 


DO     10    1=1, N 
BB(I)     =     BB(I,K1 
10    CC(I)  =    CB(K2,I) 


C 

C       LOCP    THROUGH    THE    POLES 
C 

1=0 
100    1*1*1 

IF{I    .GT.     N)     GO    TO    500 

IF(JCF(I)     .£Q.     1)     GO    TO    330 

IFfDABS  (PI  (I)  )     .LT.     1.D-10J      GO    TO    203 
C    COMPUTE     SIMPLE    COMPLEX    POLE    RESIDUES     AND    PRINT    BOTH 

RES  (I)     =    CC(I)»BB(I)      ♦    CC(I +1)»BB  (I*1) 

RESJI+1)  =CC  (I  j»BB(I*1)    -    ZZ  (I*1)*BB(I) 

I?(IPT    -EQ.    0)     GO    TO    110 

PRT  (1)  =     ELANK 

PRTJ2)     =    R2 

IFfPI  (I)     .  EQ.     0.D0)     PRT(2)      =    BLANK 

PR?  (3)     =    CS 

PRT (4)     =    ED 

WRITE  (6,90  20)     PR  (I)  ,PI(I)  ,H  ES  (I)  ,  (PRT  (J)  ,J=1,4) 

1*1*1 

PPT (3) =SN 

WRITE  (6.  9020)     PR  (I)  ,  P  I  (I)  ,  S  ES  (I)  ,  (PRT  (J)  ,J=1,4) 

GO    TO    100 
110   1*1*1 

GO    TO     130 
200    CONTINUE 
C       COMPUTE    SIMPLE    REAL    POLE    RESIDUE 


RES  (I)     =    CC  (I)*BB(I) 
IFflPT    .EO.     0)     GO    TO     100 
PRT  (1)  =     fil 


PRT  (2)=    R2 
PRT (3)=     BLANK 
?VT  (4) =     ELANK 

WRITE  (6.  90  2  0)     rH(I)  ,  P  I  (I)  ,  R  ES  (I )  ,  (PPT  (J)  ,J=1,4) 
GO    TO     100 
LOOK    AHEAD    TO     DETERMINE    SIZE    OF    THE    JORDAN     BLOCK 
300    K=1 

KT=N-I 

PO     310    J=I,KT 

IP(JCF(J)     .EQ.    0)     GO    TO    323 
310    K=K»1 
320    CONTINUE 

IFfDABS  (PI  (I)  )     .LT.     1.0-10)      GO    TO    U03 
COBPUTE    RPFEATED    COMPLEX     POLE     AND    PRINT    OUT     ALL    FOUR 
K=1 

RES  (I)=CC(I)»=B  (I)*CC(I»1)>EB(I*1)*CEfI+2)'"33(I*2)*CC{I*3)'»3B(I*3 
PES  (I  *1)  =CC(I)r 
X  BB(I*2) 

(I*2)=CC(I)»5B(I»3)*CC(I*1)>E3<I*2j 


=  CC(I)»=fi  (I)*CC(I»1)>BB(I*1)»CEfI*2)i'33(:»2)*CC{I»3)'»3B 
•1)=CC(I)»BB(I*1)-CC(I*1)»&S(I)*CC(I*2)*E-3<I*3)-CC(I»3)» 

RES  (1*2)  *CC{I)*BB(IO)»CC(I*1)>BB<I*r 
BES  ll*3)*CC|f)"B8(I*3l-CC<I*1)*B8{I*; 

IP( IPT    . EO.    0)     GO    TO    343 


PFf  (1)=P1 
PPT    2    -B2 
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340 


111 

PRT 

prt 

330    PRT 

PRT 

WRI 

9020    FOR 

9030    FOR 

* 

PRT 
1=1 
HRI 

PRT 

PRT 

IFi 

PRT 

1  =  1 
WRI 
PRT 
1  =  1 
WRI 
GO 
I  = 
GO 

C    COMFOTS 
400    CON 
KT  = 
NN'= 
DO 
NN= 
r.ES 
DO 
4  10     RES 
4  2  0    CON 
IF( 
NN  = 
PRT 
PT 
PRT 
PRT 
DO 
WRI 
NN  = 
GO 
I    = 
GO 
50  0    CON- 
RET 
END 

C 

C 

C 


U30 
uu  0 


PASS  (PS  (I))     .GT.     1.D-10)      GO    TO    330 

1)=    ELANK 

2)=     BLANK 

3}=CS 

4    =ED 
TE  (6,90  2  0)     PR  (I)  ,PI(I)  ,R  ES  (I)  ,  (PRT  (J)  ,J=1.  4) 

HAT(/,4X,'  (•  ,F13.6  ,'(  +J(  •  ,?13.6,'l  •  ,  4  X ,  '  ('  ,P13.6,  •)  •  ,318.  A  1) 
HAT(/,4X,'   (,,F13.6,,j*J(',F13.6,,|  '  ,4X, '  (• ,?13.6, ')  « , A4.I2  ,2X, 

2A8, A1) 
(3)=SN 

♦  1 

TE  (6,90  2  0)     ?R(I)  ,  P  I (I)  ,  R  ES  (I)  ,  (PRT  (J)  ,J=1,4) 

mil 

DAES(?R(I))     .LT.     1.D-13)      PRT(2)=BLANK 
(3)=    CS 

♦  1 

TE  (6,90  3  0)        PR  (I),  PI  (I)  ,  RES  (I)  ,?RT(1)  ,K,  (PRT  (J)  ,J=2,4) 
(3)=SN 

♦  1 

TE  (6.90  3  0)        PR  (I)  ,  PI  (I)  ,  RES  (I)  ,PRT  (1)  ,K,  (PRT  (  J)  ,  J  =  2,  4) 
TO    100 

r  ♦  3 

TO    100 

REPEATED    REAL    POLE    RESIDUE    AND    PRINT    OUT     ALL    K    OF    THEfl 
TINUE 

i+k-1 

0 

420   J=I,KT 

NN*1 

(J)=:ero 

410    JJ=J,KT 

(J)  =PES  (J)  ♦BB(JJ)  "CCJJJ-  NN*1) 
TINUE 

IPT    .EQ.     0)     GO    TO    440 
0 
(1)=T1 

2    =P2 
(3    =SLANK 
(4    =3LANK 
430    J=I.KT 

TE  (fc,90  3  0)     PR  (J)  ,PT  (J)  ,3  ES  (J)  ,PRT  (1)  ,  NN,  (PRT  (  J  J)  ,  J  J  =  2  ,  4) 
NN«-1 
TO    100 

KT 
TO    100 
71  SUE 
UHN 


SUBROUTINE     2ALANC  (HH,  V,  k  ,L3H,I3H, SCALE) 


INT 
REA 
REA 
SEA 
LOG 
DAT 

B2 

K  * 
L  * 
GO 


EF    I.J  ,K  ,L,.1.  N.J  J,  MH,I  GH,LOW,IEXC 

8    A  (SB,  N)  ,  SCALE  (N) 

8    C,P,G,H,3,B2,F ADIX 

e    3ACS 

AL     NOCONV 

PAPIX/Z4  21  0000  00  000  00  3  0/ 


=     RADIX    *     RADIX 
1 
N 

TO 


2  0    SCA 
IF 


100 
:::::     IN-LINE    PPOCEDUR E    FOP    PDW    AND 

COLCflN    EXCHANGE     :::::::::: 
(HI     »    J 

.EQ.     .1)     GO    TO    5  0 


DO    30    I    ■    1,    L 
T    -    A  (I.J) 
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30 

40 
50 

80 

100 

1  10 
120 
130 

mo 

150 

170 

180 
190 


A  (I, J] 
AJI.M 

CONTINUE 

■    A  d. 
=    P 

,M) 

DO    40    I    = 
F    =    A  (J 
A  (J, I) 
AJM    I 

CCNTINOE 

K.     N 

=    A  <M 

=    F 

-I) 

GO    TO     (80, 130) ,     IEXC 

::::::::::     SEARCH    FOR    ROWS     ISOLATING    AN     EIGENVALUE 

AND    PUSH    THEM    DOWN    :::::::::: 
I?     (L    .  EQ.     1)     GO    TO    280 
L   =    L    -     1 

::::::::::    FOB    J=L    STEP   -1    UNTIL    1    DO    --    :::::::::: 
DO    120    JJ    =    1,    L 
J    =    L    ♦     1    -    JJ 


CO  110  I  =  1  , 
IF  (I  . EQ. 
IF     (A  (J, I) 

CONTINUE 


J)     GO    TO    1  10 

.NE.     0.0D0)     GO    TO    120 


n   =  l 

IEXC    ■    1 
GO    TO    20 
CONTINUE 

go  to  mo 
1 


SEARCH    FOR    COLUMNS    ISOLATING     AN     EIGENVALUE 
AND    PUSH   THEM    LEFT    :::::::::: 


K    =     K    ♦ 

DO     170    J    =    K,    L 

CO    150    I    =    K,  L 

IF     (I    .  EQ.  J)     GO   TO    1  50 

IF     (A(I,J)  .NE.     O.ODO)     GO    TO    170 

CONTINUE 

K     =    K 

IEXC    =    2 

GO    TO    20 
CONTINUF 

::::::::::     NOW    BALANCE   THE    SUBMATRIX    IN    ROWS 
DO    180    I    =    K,     L 
SCALE(I)     =     1.0D0 

::::::::::     ITERATIVE    LOOP   P OR    NORM    REDUCTION 
NOCCNV    =    . FALSE. 

DO    270    I    =     K,     L 

C    =    O.ODO 
S    =    C.ODO 


K    TO     L 


200 


210 


220 
230 


DO    200    J    *    K, 
IF     (J    .EQ. 

c  ■  c 

p   =   p 

CONTINUE 

:::::::    GUARD    AGAI NST    Z 

IF     (C    . ED.     O.CDO    . OR.     R 

G    =    R    /    RADIX 

F     «     1.0D0 

£     «    C    ♦     R 

IP     (C    .  GE.     G) 

F    »    F    *     RADIX 

C     »    C    »     B2 

GO    TO    210 

G    =    F    »     RADIX 

IP     (C    . LT.     G) 

F    *    P    /    RADIX 


L 

I)     GO    TO    2  00 
DABS  (A  (J,  I)  | 
DABS (A  (I,J)I 

I  RO  C 

.EQ. 


GO  TO  220 


OR  R  DUE  TO  UNDERFLOW 
0.3D3)  GO  TO  273 


GO  TO  240 


80 


2U0 


250 


260 
270 


280 


C    =    C    /    B2 

GO    TO    230 

:::::::    NOW    BALANCE 

IF     (<C    +    R)     /    P    .GE. 

G    =     T.0D0    /    P 

SCALEJI)     =    SCALE  (I) 

NOCONV    =    .TRUE. 


0.  9  5D0    >    S)     30    TO    270 

>    P 


DO    250    J 
A(I,J)     = 

CO    260    J 
A(J,I)     ■ 

CONTINOE 

IF     (NOCONV) 

LOW     =    K 

IGH     =    L 

RETURN 

end' 


=    K,     N 

MI. J)    * 

»    1.    L 

A  (J, I)     * 


GO    TO    190 


90 


100 


LAST    CARD    OP    3ALANC 


SUBROUTINE    ORTH  ES  (NB,  N  ,LOW  ,  IGH,  A,  ORT) 

INTEGER     I,  J,fl,N,II,  JJ  .  LA,MP,NM,  IGH.KP1  ,  LOW 
REALf8     A  (NM,  N)  .ORT  (IGH) 
REAL»8    F.G.H,  SCALE 
REALMS    DSQHT, DABS, DSIGN 

LA    =     IGH    -     1 

KP1     *    LOW    ♦     1 

IF     (LA    .LT.     KP1)     GO    TO    200 

DO    180    K    =     KP1,     LA 

K    =    O.ODO 

GRT(n)     =    O.ODO 

SCALE    =    O.ODO 
::::::::::     SCALE    COLUMN     (ALGOL    TOL    THEN    NOT    NEEDED) 

CO    90    I    =    H.     IGH 

SCALE    =     SCALE    ♦     DA  ES  (  A  (I  ,  tl-1 )  ) 


1  10 


120 
130 


0.0  DO) 


0    TO    180 

-1     UNTIL 


IP     (SCALE    .EQ 

BP    =     B    ♦    IGH 

:::::::     FOR    I=IGH    STEP 

DO    100    II    =    n.    IGH 

:  *  bp  -  li 

ORT(I)     =    A(I.B-I)     /    SCALE 
H    =    H    ♦    ORT  (I)     a    ORT  (I) 
CONTINUE 

G     =    -DSIGN  (DSQRT(H)  ,DPT(  B)  ) 

H    =    H    -     ORT(fl)     •    G 

ORT(B)     *    ORT  (B)     -    G 

:::::::    FOP.B     (I-(UrtUT)/H)    *    A 


B     DO    -■ 


N 


DO     13  0    J    =    B , 
P    =    O.ODO 

:::::     FOR    I*IGH    STEP   -1    UNTIL 
DO     110    II    =    B,     IGH 

I     =     .IP    -    II 

F    =    P    ♦    OPT(I)     •    A   (I, J) 
CONTINUE 


«     DO     -• 


F    =    P    /    H 

DO    120    I 
A(I.J)      = 

CONTINUE 


*     B,     IGH 

A  (I,  J)     -    ?    •»     ORT  (I) 


81 


1U0 


:::;:::    FOfia     (I- (U^OT) /H)  *&»  (I-(U>  UT) /H) 
DO     160    1=1,     IGH 

F    =    0.0D0 
:::::::     POR    J  =  IGH    STEP   -1    UNTIL    H    DO    -- 
DO    140    JJ    =     H.     IGH 
J     =    MP    -    JJ 

F    =    F    ♦    ORT(J)     5    A  (I, J) 
CONTINUE 


150 
160 


180 
100 


F    =    F    /    H 

DO    150    J    =    H,     IGH 
A(I,J)     =    A(I,J)     -    F    * 

CONTINUE 

ORT(M)     =    SCALE    *    ORT(«) 
AfH,M-1)     =    SCALE    »    G 
CONTINUE 


ORT  (J) 


RETURN 

end"  "" 


LAST    CARD    OF    ORT HES 


SUBROUTINE    CRT?  AN  (NH,  N,LOW,  IGH,  A,  ORT,  Z) 

INTEGER     I,  J,N,  KL,  MM,  HP,  N M, I GH ,LOW , MP1 
REALMS    A  (fl(1,  IGH)  ,  ORT  (IGH)  ,  Z  (NH,  N) 
REALMS    G 

::::::::::    INITIALIZE    Z    TO    IDENTITY    HATRIX 
DO    80    I    =    1,     N 


60 
80 


DO    60    J 
Z{I,J)     « 


o.5do 


100 


1  10 


120 
1  30 

mo 

200 


Z(I.I)     =    1.0D0 
CONTINUE 

KL    =     IGH    -     LOW    -     1 
IF     (KL    . LT.     1)     GO    TO    200 
::::::::::     FOR    ar=IGH-1    STS 
DO    140    Mfl    =    1 ,     KL 
HP    *     IGH    -    HH 
IF     (A  (HP, HP- 1)     .  EQ. 
H  P  1    =    HP    ♦     1 

DO     100    I    *    HP1.     IGH 
ORT(I)     =    A  (I,HP-1) 


1     UNTIL     LOW*1     DO 
0.0DO)     GO    TO     140 


DO    130    J    =    HP, 
G    =    0-0D0 


IGH 

■    110    I    «    BP 


IGH 


G    =    G    ♦    ORT  (if    *    Z  (I,  J) 
:::::     DIVISOR     hlLOW    IS     NE3ATI 


:VE    OF    H     FOP-ED     IN    ORTHFS. 
DOUBLE    DIVISION     AVOIDS    POSSIBLE    UNDERFLOW     :::::; 
(G    /    OhT(KP))     /    A  (HF,  SP-1J 


DO    120    I 
Z(I,J)     ■ 


*     HP,     IGH 
Z(I,J)      ♦ 


z  *  opr  (i) 


CONTINUE 
CONTINUE 
RLTU8H 
END*  ' 


LAST    CARD    OF    ORT  RAN 
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—  .ER?~ 


SUBROUTINE    HQR2  (NM  ,  N,  LOW  ,1 Z  H  ,  H,  BR,  WI, 

INTEGER    I,JrK.L,H,H»E»,II,JJ,LL,flB,Na 

X  IGH  , ITS, LOW  ,HF2,  ENM  2,IERR  -»»•'.. 

F.EAL*8    H(NK.N)  ,  WR  (N)  ,  WI  (N)  ,  Z(NM,  N) 
REAL*8    P,Q.R,S,T,W.X.  Y,3A,S  A,  VI  ,VP.,Z2       a  -  -  „ 

REALMS    DSQKT,  DABS,DSIGN  "      "" * ICHEP 

INTEGER    KINO 

LOGICAL    NOTLAS 

COMPLEX=-16     Z3 

CCXPLEX^16     DCMPLX 

REALrt8    UREAL, DI.1AG 

::::::::::     STATEMENT    FUNCTIONS    ENABLE      rx„0 

IMAGINARY    PARTS    OF    DOUBLE    PRECISION    £"'»£- 2'*  ^TIOM    OF 

DREAL(23)     =    Z3  LUMBERS 

DIMAG(Z3j     =     (0.  0D0.-1  .0D0)     •    Z3 

DATA    KACHEP/Z3K100000000000  00/ 

IERR    =    0 
NORM    =    O.ODO 
K    =     1 

::::::::::     STORE    ROOTS    ISOLATED    BY    B\ 
AND    COMPUTE    MATRIX    NORM     :; 
DO    50    I    =     1,     N 


:al   AND 


A  SC 


UO 


WR  (I 
WI 
50    CONTI 


DO    UO    J    =    K,     N 

NORM    =    NORM    +    DABS(H(I,J)) 


K    =    I 

IF     (I    .GE. 


a: 


LOW 

o.u&o 


AND.    I    .  LE.    IGH)      ;o    ro 


EN    =    IGH 
T    =     O.ODO 

60    IF*  "(EN*  '.Li. 
ITS    =    0 
NA    =     EN    -     1 
ENM 2     =    NA    - 


SEARCH 

LOW) 


OR     NEXT 
GO    TO    340 


EIGENVALUE'S 


LOOP    FOR    SINGLE     SMALL    SUl'-niir  , 

FOB    L  =  FN    STEP    -1     UNTIL    Li'n     p,,    l   K*L    ELEMENT 


70    DO    80    LL    =     LOW,     EN 

L    =    EN    ♦     LOW    -     LL 
IF     (L    .  EQ.     LOW)     GO    TO    100 
S    =    DABS  (H  (L-1,  L-1  )  )     ♦    DABS(H(L,L|i 
"Q.     O.ODO)     S    =    NOPM 
AES  (H  (L.L-1)  )      .LE.      MACHEP    »     S) 


O       -       VI 

IP     (S 

IF     [DJ 
JNTINUE 


8  0    CON 

::::::::::     FORM    SHIFT    :: 
100    X    =     H  (EN, EN) 

IP     (L    . EC.     EN)     GO    TO    270 
Y    =     H  (NA.NA) 
W    =     H  (EN,NA       *    H  (NA,EN> 
IF     (L    .EQ.     NA)     GO    TO    280 
ITS     .EO.     30) 


110    TO     100 


IF 
IF 


ITS 

r' 


GO    TO    1OO0 
NE.     10'  .  AND.     ITS    .  NE.     20) 
FORM    EXCEITIONAL     SHIFT 


>1     TO 


no 


120 


130 


DO    120    I    =     LOW, 
H(I,I)     =    B(I,IJ 


OABS  (H(P.N.NA)  )     ♦    DABS{«  (1»A,  ENS21 1 

0.7500    *    S 

S 


S  = 
X  » 
T    =     X 

W    =    -0.H375D0    f    S    » 
ITS     «    ITS    ♦     1 
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::::::::::    LOOK    FOE   T«0   CONSECUTIVE   SMALL 
SUE-DIAGOVAL    ELEMENTS- 
FOB    H=SN-2     STEP     -1    UNTIL    L    DO    - 
DO     140    MB    =    L,     ENM2 
M    =    ENM2    *    L    -    MM 
ZZ    =    H(B.M) 
R    =    X    -     ZZ 
S    =    1    -     ZZ 

H(S,M  +  1) 


1U0 
150 


160 


S    =    DABS(P)     ♦    DABS  (Q)     ♦     DAES(R) 
P    =    P    /    S 
Q    =   Q   /    S 
R    =    R    /    S 

IF     (M    . EQ.     L)     GO    TO    150 
IF     (DABS  (H  (K,B-1)  )     *     (DABS(Q)     ♦ 
C  '*      (DABS  (H  (M- 1,fl-1))     ♦    DAPS(ZZ) 

CONTINUE 


DABS(R))      .LE.     KACHEP    t     DABS(P1 
♦     DABS  (H  (fl+1,  a*  1>  )  )  )     GO    TO     150 


BP2    =    B    ♦    2 

DO    160    I    =     BP2.     EM 
H(I,I-2)     =    O.ODO 
IF     (I    .EQ.     BP2) 
HfI.I-3)     =    0.0D0 

CONTI  NUE 


GO    TO    16  0 


170 


180 
190 


200 
210 


220 
230 


DO 


DOUBLE    QR    STEP    INVOLVING    ROWS    L    TO    EN 

COLUMNS    B    TO    EN     :::::::::: 
260    K    =    B,     NA 
NOTLAS    =    K    .  NE.     NA 
IF     (K    .EQ.     H)     GO    TO    170 
P    =    H  (K,K-1) 
Q    =    H  («♦  1,  K-  1) 
R    =    0.0D0 

IF     (NOTLAS)     R    =    H(K*2,K-1) 
X    =    DABS  (P)     ♦    DAES(Q)     ♦     DABS(R) 


AND 


0.0D0)     GO    TO     260 


S     =    DSIGN(DSQRT(Fvp*n»o*  R-»R)  ,P) 

IF     (K    . EQ-     Mf    GO    TO    180 

H  (K.K-1)     =    -5    •     X 

GO    TO    190 

IF     (L    -NE.     B)     H(K,K-1)     =     -H(K,K-1) 

P    =    P    ♦     S 

X    =    P    /    S 

Y     -    Q    /    S 

ZZ    =    P    /    S 

Q    ■    Q    /    P 

r     =    h    /     P 

:::::::     ROH    MODIFICATION     ::::::::: 

DO    2  10    J    =    K.     N 

P 

IF 

'[^♦2^5)     -     P 

U  :   p 


u    j    =    :\ ,     n 

=    H  (K.J)      ♦     0    a    H  (K»-  1.  J) 

(.NOT.     NOTLAS)     GO     TO    200 
=    P    ♦    B    »    H  IK+2.J) 
H(K»2,J)     =    H(K*2 
H (K» I.J)     «    H  (K*  1 
H?K,J)     ■    H(K,J) 
CONTINUE 


J    =    r.INO  (EN.K  +  3) 

:::::::     COLUMN    MODIFICATION    :::: 

DO    230    I    =    1,    J 

P    =    X    *    H  (I  .  K)     ♦    T    *     H  (I.  K»  1) 
IF     (.NOT.     NOTLAS)     GO     TO    220 
P    *    P    ♦    ZZ    •    H  (I,K+2( 
H (I,  K*2)     =    H (I,  X*2)     -     P    >     R 
K  (I, K*1)     ■    H (I, K*li     -     P    *    Q 
H/I.K)      -     rt(I,K)      -    P 

CONTINUE 
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2U0 
250 
260 

270 


::::::::::    ACCUMULATE   TR ANS FORMATION? 
DO    250    I    =    LOW,     IGH 

P    »   X   *    Z  (I.KJ     ♦    T    *     2(1-  K*1) 
IF     (.NOT.     N&TLAS)     GO    TO    2U0 
F    =    P    ♦    ZZ    5    Z(I,K*2| 
Z  (I,  K+2)     =    Z(I,  K+2)     -     P    *     H 
Z (I, K*1)     =    z|l,  K«-1J     -     P    »     Q 
ZJI,K)     =    Z(I,K)      -    P 
CONTINUE 

CONTINUE 

GO    TO    70 


)0)      WR  (EN) 


X    -     H    /    ZZ 


ONE    ROOT    FOUND    : 

H  (EN. EN)     =     X    ♦    T 

WR (EN)     =    H  (EN. EN) 

HI (EN       =    0. 0D0 

EN    =    NA 

GO    TO    60 

::::::::::    TWO    ROOTS    FOUND 
280    F    =     (T    -    X)     /    2.0D0 

C   =    P   »    P    ♦    W 

ZZ    =     DSQRT  (DABS  (Q) ) 

H(EN.EN)     =     X    ♦    T 

X    =     H  (EN, EN) 

K  (NA,  NA)     =     Y    ♦    T 

IF     (Q    .LT.     0.0D0)     GO    TO    323 

::::::::::     REAL    PAIP    :::::: 

ZZ    =     P    ♦    DSIGNfZZ,P) 

WR(NA)     =    X     ♦    ZZ 

WR(ENi     =    WR  (NA) 

IF     (ZZ    -NE.    O.DDC 

WI(NA)     =    0.0D0 

WI(ENJ     =    0.0D0 

X    =     H  (FN.NA) 

S    =     DABSfX)     +    DABS(ZZ) 

P    =     X    /    S 

0   =    ZZ   /   s 

h    =     DSQRT  (T'*'P  +  Q*Q) 

P    =     P    /    R 

Q  ■    Q   /    H 

::::::::::     ROW    MODIFICATION     ::::::::: 

DO    290   J    =    NA,    N 
ZZ    =    H(NA.J) 

H(NA,J)     *    Q    *    ZZ    ♦    P    *    H(FN,J) 
HJEN.J)      =    Q    e    H(EN,J)     -     P    9    ZZ 
Mil NUE 

::::::::::    COLUMN    MODIFICATION    :::::: 

DO    300    I    ■     1.    EN 
ZZ    ■    M  ( I     NA) 

H(I,NA)     '=' Q    *    ZZ    ♦    P    *•    Hfl.ENl 
H  jl.  EN)      =    Q    •    H  (I,  EN)     -     P    6     ZZ 

CONTINUE 

::::::::::     ACCUMULATE    TR ANS P0RMATIUN5 

DO    310    r    -    LOW,     IGH 
ZZ    =    Z  (r,  NA) 

Z(r,NA)      =    Q    *    ZZ    ♦     P    5    Z(r,EN) 
ZJI.EN)      ■    G    •    Z(I,  EN)     -     P    *    ZZ 

CONTINUE 

GO    TO    3  30 


290    CON 


300 


310 


320 


330 


WR(NA 
W  R  (  E  N 
WI(NA 
WI(EN 
EN     = 


GO  70  60 


:::  complex  PAIR 
=  X  ♦  P 
=  X  ♦  ? 

=    ZZ 
=    -ZZ 
^NM2 


3U0    IF     (NORM 


ALL    ROOTS    POUND.        3ACKSU&5TITHTP    -n    rrwn 
VECTORS    OF    UPFSa     TRIANGULAR     ?f,\im     '..     'ISD 
:C.     O.ODO)     GO    TO     1001                                n     ....:::: 
FOR    EN=N    STEP    -1     UNTIL    1     DO    --     


85 


DO    800    NN    =    1, 
EN   *    M   ♦    1    ■ 

P    =    VR(EN) 


600 


610 
620 


630 


640 


650 
700 


710 

720 
730 


NN 


IF     (Q)     710.     600,    800 

:::::::     REAL    VECTOR    :::::::::: 

K      —      FN 

HjEN.EN)     =     1.0D0 

IF     (NA    .  EQ.    0)     GO    TO    800 

:::::::     FOR    I=EN-1    STEF    -1    ONTIL    "       ^n    - 

DO    700    II    =1,     NA 

I    =    EN    -    II 

W    =    H  (1,1)     -    P 

R    =    H(I.EN) 

IF    (M    .GT.     NA)     GO    TO    620 

DO    610    J    =    H,     NA 

R    =    R    ♦    H  (1,3)     "    H  (J,  EN) 

IF     (WI(I)     .GE.     O.0D0)     GO    TO    63.: 

ZZ    =     W 

S    =    R 

GO    TO    7  00 

H    *    I 

IF     (WI(I) 

T  =   a 

IF     (W    .EQ.     0.0D0) 
H  (I,  ENf     =    -R    /    T 
GO    TO    700 
:::::::     SOLVE    REAL    EQUATIONS 
X    =    H  (1,1*1 


NE. 

0 
/ 


0.0D0) 

T    = 


GO    TO    64.; 
ttACHEP    »        s^ri, 


T    =    H  (1*1,1) 

0    =     (WP  (I)     -     P)     •     (WR  (I)     -    P) 

T    =    JX    »    S    -     ZZ    »    H)     /    Q 

K(I,  EN)     =    T 

IF     (DABS(X)      .LE.     DABS  (ZZ)  )      GO 

H(I*1  ,EN)     =     (-P    -     W    »     T)     /    X 

GO"    TO    700 

H  (I*  1  ,EN)     =     (-S    -    Y    *     T)     /    ZZ 
CO  NT  I  NU  E 

:::::::     END    PEAL    VECTOP     ::::::::: 
GO   TO   800 

:::::::    Cuf!PLi.X    VECTOR    :::::::::: 
«    =    NA 


WI  [l\ 
650 


"I  (I) 


:::::::  LAST  VECTOP  component  Cti:~  ~v  ti.- 
"IGENVECTOP  HATRIX  IS  TRI " . v ;UI » 2  * 
IP  (DALS  (H  (EN,NA)  J  .  L  E.  DABS  (H  (NX.  "u  ,  K_l 
H  (t.A.NA)  =  Q  /  H  (LN.MA) 
HfNA.ESi  "  -(H(EN,EN)  -  P)  /  H(fS. 
730 


/    DC 


H (NA, NA 

GO   t6 

Z3    =    DC.IPLXfO.  ODO.  -H  (VHt  EN)) 

h(Na,:;a>    *   dreal(Z3) 
!i(na,  en)    =   di-ag(z3) 

HfEH.NA       =    0.0  DO 

H    EN, EN       =    1.0  DO 

ENH2    =    NA    -     1 

IF     (EN«12    .  EQ.     0)     GO    TO    3  00 

:::::::     POP    I=EN-2    STEP    -1    UNTIL 

DO    790    II    =     1.     ENH2 

I    -    HA    -    II 

«    =    Hfl.I)     -    P 

SA     =     O.ODO 

5A    =     H(I,EN) 


)  )  )      GC 

VA) 

r~*  (M  {HK 


NARY     5 
"fo" 72 


0    THAT 

6" " 


.NA)  -P.Q) 


r»o 


760 


DO    760    J    =  !!,     N  A 

PA    =    RA  ♦     H(I,J) 

SA    *    SA  ♦     H(I»J) 

CONTINUE 


H  (J,  NA) 
H  (J,  EN) 


IF     (WI(I) 

zz  =   y 


.GE.     O.DDDl      GO    TO    7' 


86 


770 


780 


R    = 

■    R 

S    = 

i    S 

GO 

TO 

(1    = 

i    I 

IF 

(W 

Z3 

if 

TO 

X    = 

'    H 

Y    = 

■    H 

VR 

= 

VI 

= 

IF 

(V 

85 


90 


Z3  = 
H(I,N 
HJI,E 
IF  (D 
H  (I*  1 
H  II*  1 
GO  TO 
23  = 
H  (1*1 
H  (I*  1 
CONTINUE 


800    CONTINUE 


DO    8«0    I    = 
IF     (I    .G 

DO    82  0    J 
2(1. J)     = 


!20 

IU0    CONTINUE 


A 

A 

790 

1(1)     -NE.     0.0D0)      GO    TO    780 
DCMPLX  (-RA,-SA)      /    DCMPLX  (W,Q) 
A)     =    DREAL  (Z3) 
Hi     =     DIM  AG    Z3 

790 
SOLVE    COMPLEX    S3UATIONS    :::::::::: 
(1,1+1) 

1*1,1 
(WR(I)     -    P)     *     (WR(I)      "    P)     ♦     MI(I)     *    «I(I)      *    Q    *    Q 
(WR(I)     -    P       >    2.  0D0    *    Q 

P    .EQ.     0.0D0     .AND.     VI    .SO.     0.0D0)     VR    =    MACHEP    *    NORM 
ABS(S)     ♦    DABS(Q)      ♦    DABS(X)      ♦     DABS(Y)      *     DABS(ZZ)) 
DCMPLX  <XCR-ZZ^3A*Q*SA,X*S-ZZ''SA-S;'>FA)     /    DCMPLX  (VR ,  VI) 
A)     =     DREAL  (Z3) 
Nl     =     DIM  AG  (Z3J 

ABS(X)      .LE.    DABS  (ZZ)      ♦    DABS(Q))     GO    TD     785 
,NA)     =     (-RA    -WO     H(I,NA)     ♦    Q    9     H(I,EN1)     /    X 
,ENJ     =     (-SA    -WO     H(I,2N)     -    Q    *     H(I,NA)j     /    X 

790 
DCHPLX(-B-Y*H(I,  NA),-S-X*H(I,EH))     /    DC.-.FLX  (ZZ,  Q) 
,NA>     =    DREAL(Z3) 
,EN)     =    DIMAG(Z3| 

END   COMPLEX   VECTOR    :::::::::: 

END    BACK    SUBSTITUTION. 

VECTORS    OF    ISOLATED    ROOTS    :::::::::: 

1,     N 

E.     LOW    .AND.     I    .  LE.     IGH)      30    TO    8U0 

■    I.     » 
H  (I, J) 


860 


DO    880    JJ    = 
J    =    N    ♦ 
M    =    .1IN0 

DO    88  0    I 
ZZ    - 

DO    86 

ZZ    = 


MULTIPLY    BY    TRANSFORMATION     MATRIX    TO 

VECTOPS    OF    ORIGINAL    FULL    MAThlX. 

POR   J=N    STEP    -1     UNTIL    LOW    DO    —    ::::: 

LOW,     N 
LOW    -    J J 
(J, IGH) 

=    LOW,     IGH 
0.  0D0 

0    IC    =    LOW.     M 

ZZ    ♦     2  (I,K)     *    H(K,J) 


;ive 


Z(I,J] 
180    CONTINUE 


ZZ 


GO    TO    1001 


0  0     I  ERR     ■*    EN 

01  RETURN 


END 


SET    ERROR    —     ND    CONVERGENCE    TO    AN 
EIGENVALUE    AFTER     30    ITERATIONS     :: 


LAST    CARD    OF    H^H  2 


SUBROUTINE     2AL3AK  (NM,  N.LOW,  IGH,  SCALE,  M,  Z) 

INTEGER     I,  J,K,n,N,II,  N«,:-.H  .LOW 
PFAL*8    SCALS(N)  ,Z(NK,  M) 
REAL"8    S 


IF 

IF 


(B_-£Q 


0)     GO    TO    200 
IGH     -EQ.     LOW)     GO    TO    123 


87 


100 

no 
120 


DO    110    I    =    LOW,     IGH 

S    =    SCALE(I) 
::::::::::     LEFT    HAND    EIGENVECTORS     ARE    3ACK    TRANSFORMED 
IF    THE    FOREGOIN3     STATEMENT     IS    REPLACED    BT 
S=1.0D0/SCALE  (I)  .    :::::::::: 
DO    100   J    =    1 ,    fl 
Z  (I, J)     =    2  (I,J)     »    S 

CONTINOE 

:::::::::    FOR    I=LOW-1    STEP    -1    UNTIL    1, 

IGH*1    STEP    1    UNTIL    N    DO    --    :::::::::: 

DO     140    II    =    1,     N 
I    =    II 
IF     (I    .GE.     LOW    .AND.     I 


IF 


I    =    LOW     -    II 


{I    .  LT.     LOW) 
K    =    SCALE  (II 
IF     (K    ,2Q.     I)     GO    TO    140 


LE.     IGH)      30    TO     140 


130 
140 
200 


DO    13  0    J    =    1, 
S    =    Z(I,J) 

=    Z(K,J) 
S 

cont: 


3      -      i,   \x  ,* 

ZJK.J       = 
IT  I  NO  E 


CONTINUE 
RETURN 

end"  ' 


LAST    CARD    OF    SAL BAK 


SUBROUTINF     HQR(NM,N,LOW,I"H,H,WR,WI,rESR) 


INT 

RFA 
REA 
REA 
INT 
LOG 


EGER    I,J,K,L,B,N.EN,LL,  B fl, NA , Nfl, 13 H , ITS  ,  LOW , BP2, ENB2 , I  ERR 

L«8    H  (nK.Nf,  WR(N)  ,  WI(N| 

L"8    P,0,R,5.T,W.X,Y,ZZ,NORB,  BACHEP 


L*8    DSCKT, DABS, DSIGN 
"~R    nlNO 


EGE 

ICAL    NOTLAS 


DATA    F1ACHEP/Z34  10000000000300/ 

IERP    =0 

NORfl    =    0.0  DO 

K    =     1 

::::::::::     STORE    ROOTS   ISOLATED    BT    BALANC 

AND   COBPUTE   MATRIX    NORfl    :::::::::: 
DO    50    I    »    1,    N 

DO    40    J    =    K,     N 
40  NCRK    =    NORB    ♦    DABS(H(I,J)) 

K    =    I 

IF     (I    .GE.     LOW     .AND.     I    .  LE.     IGH)     30    TO    50 

WR/Il     =     H(I.I) 

WI  M     =     O.O&O 

TTNUE 


50    CON 

EH    =     IGH 
T    =     0.000 

::::::::::    SEARCH    FOR    NEXT    EIGENVALUES    :::::::::: 
60    IF     (EN    .LT.     LOW)     GO    TO    1031 
ITS    =    0 
NA    =     EN     -     1 
EN.M2    =    NA    -     1 

::::::::::     LOOK    FOR    SIVr.LE    SMALL    3U3- CT  AGOTtAL    ELZM2MT 
FOR    L=£N    STEP    -1     UNTIL    LOU    PO    —    :::::::::: 
70    DO    30    U    '     LOW,     EN 

L    *    FN    ♦    LOW    -     LL 

IF     (L    .20.     LOW)     GO    TO    13  0 


88 


S    =    DABS  (H  (L-1.L-1))     ♦    D  ABS  (  H  (L,  L)  ) 
IF     (S    .EQ.     O.ODO)     S    =    NORM 

IF     (DABS  (H  (L,L-1))     -LE.     HACHEP    »    S)     GO      -«     lnn 
80    CONTINUE  -      TOO 

::::::::::     FORM    SHIFT    :::::::::: 
100    X    =     H  (SN.EN1 

IF     (L    .  EQ.     EN)     GO    TO    270 

Y  =    H  (NA,NA) 

W    =     H  (EN.NA       «■    H(NA.EN) 

IF     (L    .  EQ.     ail     GO    TO    280 

IF     (ITS     .EQ.     30)     GO    TO    1000 

IF     (ITS    .HE.     10    .AND.     ITS    .  NE.     20)     GO    TO    »   si 

::::::::::    FORM    EXCEPTIONAL    SHIFT    ::::::::   :. 

T  =    T    ♦    X 

DO    120    I    =    LOW,     EN 
120    H(I,I)     =    H  (1,1)     -    X 

S    =    DABS  (H  (EN.NA)  )     ♦     DABS (H  (NA, ENM2) ) 
X    =    0.75D0    O    § 

Y  =    X 

W    =    -0.U375D0    »    S    *    S 
130    ITS     =    ITS    ♦     1 

::::::::::     LOOK    FOR    TWO   CON  SECUTIVE    SMALL 
S'JB-DIAGON  AL    ELEMENTS. 

FOR    H=EN-2     STEP     -1     UNTIL    L     DO    --     

DO    140    MM    =    L,     ENB2  

M    =    ENM2    ♦    L    -    MM 

ZZ    =     H(M,M) 

R    =    X    -     ZZ 

S    =    Y    -     ZZ 

P    =     (R    *    S    -     W)     /    H(H*1,M)     ♦     H(3,H»1) 

Q    =    H  (N*1,MO)     -    ZZ    -    R     -    S 

R    «=    H(M*2,H*1) 

S    =    DABS(P)     ♦     DABS  (Q)     ♦     DABS(R) 

P    =    P    /    S 

Q    =    0    /    S 

R    =    R    /    S 

IF     (H    . EQ.     L)     GO    TO    150 

IF     (DABS?H(r,M-1))     5     (DABS(Q)     *    DABS(R))         ,g       -.rMFp    »     nmc/n. 

iu0xcoNTiN]EDAbslHl""1"','ln  *  DAB^-Z)  *  M"<UH:«5ffnf  SoDJ5s8l 

150    MP2    =    M    ♦    2 

DO     160    I    =    MP2.     EN 

Hfl,I-21     =    O.ODO 

IF     (I    .EQ.     MP21     GO    TO    15  0 

H(I,I-3)     =    O.ODO 
160    CONTINUE 

::::::::::     DOUBLE    QR    STEP   INVOLVING    Rows    i     Tn    rw     i  «n 
COLUMNS    M    TO    EN     ::::::::::  '    iU    £,M    AND 

DO    260    K    =     M,     NA 

NOTLAS    =    K    .  NE.     NA 

IF     (K    .  BO.     M)     GO    TO    170 

P    =    II(K,K-1) 

Q    =    H  (K*  1,X-  1) 

R    =    O.ODO 

IF     (NOTLAS)     R    =     H(K*2,K-1) 

X     ■    DABS(?)     ♦     DAES(Q)      ♦     DABS(P) 

IF     (X    .23.     O.ODO)     dO    TO     260 

P    »    P    /    X 

Q    =    Q    /    X 

h    =    R    /    X 
170  S    *    DSIGN(DSQRT(P3  {•♦V7*  F-'R)  ,P) 

IF     (K    . EQ.     Ml     GO    TO    18J 

H(K.K-I)     =    -5    •     X 

r,d    TO    190 
1°0  IF     (L    -NE.     M)     H(K,K-1)     *     -H(K,K-1) 

1 90  P    ■    P    ♦     S 

X    «  ?  /  s 
y   »  y  /  s 


89 


Z2.    =    H    /    S 

Q    =    Q   /    ? 

R    =    R    /    P 

:::::::    ROW    MODIFICATION     :::::: 

DO    210    J    =    K,     SN 

P    =    H  (K,J)     ♦    Q    v    9  (K»  1,  J) 
IF     (.NOT.     NOTLAS)     GO    TO    200 
R    a    H  (K+2, 


200 
210 


P   =    P 
H  (K*2,J) 
H(K+1,J)     ■ 

HjK.J)     =    H( 
CONTINUE 


H  (K+2,  J) 
H(K*2,J)     - 
H(K*1,Ji    - 
K,J)     -    P    > 


-     P    *    ZZ 
P    *     I 
X 


J    =    BIN0(EN,K*3) 

COLUMN    MODIFICATION 


DO  230  I  =  L,  J 

P  =  X  0  H  (I.KJ  ♦  I  »  H  fI.K  +  1) 
IF  (.NOT.  NOTLAS)  GO  TO  220 
F  =  P  ♦  ZZ  <"  H(I,K*2) 


220 
230 
260 

270 
280 


CONTINUE 
CONTINUE 
GO    TO    70 

"  "x 


H (I,  K*2)  =  H (I, K*2) 
H  ( I ,  K  ♦  1  j  =  H  ( I ,  K+  1  j 
HJI,  K)     =    H(I,K)     -    ? 


P    *     H 
?    *    Q 


WR(EN) 

WI(EN)     =    0.0D0 

EN    =     NA 

GO    TO    60 


ONE    FOOT    FOUND 


>)     GO 
PAIR 


TO    323 


TWO    ROOTS    FOUND 
P    =     <Y    -    X)     /    2.0D0 
Q   ■    P   e    P   ♦    V 
2Z    =     DSQRT  (DABS  (Q)  ) 
X    =     X    ♦    T 
IF     (Q    .LT.     0.0D0) 
::::::::::     R  Z  AL 
ZZ    =     P    ♦    DSIGNfZZ.P) 
WR(NA)     =    X     ♦    ZZ 
WR(ENi     =    WR  (NA) 
IF     (ZZ    .NE.     0.0D0)      HR  (EN)     = 
WI(NA)     =    0.0D0 
WI(EN)     =    0.0D0 
GO    TO    330 

COMPLEX    PAIR    ::: 


X    -     H    /    ZZ 


320    Wfi(NA)     =    X    ♦ 


WR/EN       =    X 


330 


WI(!IA 
«I(EN 

EN    =     ENM2 
GO    TO    60 


=    ZZ 
ZZ 


1000 
1001 


IERR     =    EN 
RETURN 

Eiib" 


SET    ERROR    —    NO     CONVERGENCE    TO     AN 
EIGENVALUE    AFTER     30    ITERATIONS     ::: 


LAST    CARD    OF    HQB 


SUBROUTINE    PSDCAL (N2, NS, FA  .  X. NC , GW , Z V    ,C, HO.HT-HU . H. 

1  F3GE,NG  ,GAH, ACL,P,WR ,«I,Dl  , D2, JCF, S SS , Q, S ,  Bo, ZZ . I TU , 

2  IPSD.INORM) 

PSDCAL    COMPUTES    THP    FSD     OP    OUTPUTS    OR     CONTROLS    OF 
A    CONTROLLED    SYSTEM 

IYO»     1  OUTPUT    PSD 

*     2  CONTROL    PSD 

IPSD-1  PSD 

«2  PSD    AND    TF     RESIDUES 


90 


c 
c 

C                                INORM=                1-2,...     N3        NORMALIZED     3Y    ITH    PROCESS     NOISE 
C  NG*1, NG*NO    NORMALIZED    EY    ITH     MEAS    NOISE 


C 


DOOBLE    PRECISION    FA.X,GW,~V . C,H Y, H, F8GE , GAM , ACL, P, HE . WI , D 1 , D2 , RES 
1    BS,CC,Q.R,PSD. W,DNOFM,DN1 ,  SMAX,ZLOG,  EMOD, DW, ST, OM , R E, A  I, HO, D W 1 
C0MPLErsi6_CD,ZN    " 

1 
2 
3 


W(  30)  ,  3B(N2)  ,C 
INTEGER  JCFJN2) 
DATA     DW1/1.D0,2 

IF(IYO  .EQ. 
IFflNORM  .  E 
IPT    =    0 


DW 1/1. DO, 2. DO, 5. DO, 10. DO/ 
C 

0)     IY0=1 
EQ.     0)      INORH    =    1 


IF(IPSD     .GT.     1)      IPT    =     1 
C 

II    =     INORtl    -    NG 


IFfIX    .GT.     01     WRITE  (6.8000)      IX 
8000    FOFMAT(/'     SUBSEQUENT    PSD    IS     NORMALIZED    BY    MEAS    NO.', 13) 

IPfIX    .LE.     0)     WRITE  f6  .8010)      INORN 
8010    FORKATf/'     SUBSEQUENT    PSD    IS     NORMALIZED    BY     PROCESS     NOISE 
NSQ    =    N2=NN2 
C 

C  ::::::::::    COMPUTE    EIGENSYSTEM   OF   CONTROLLED    SYSTEM 

C 

C  ::::::::::    FORM    FA 

DO    10    I=1,NS 
DO    10    J=1, NS 
FA(I.J)     =    ACL(I.J) 
10    F  A(NS*I,  J)     =    0.  DO 
DO    20    1=1, NS 
DO    20    J=1,NS 
ST    =    0.D0 
DO    15    K=1,NO 
15    ST    =    ST    ♦    FBGEfI,K)  e-H  (K,J) 

FA(I,NS*J)      -    -ST 
20    FA(NS*I,NS  +  J)     =    F(I.J)     -    ST 

CALL    RA^FNT(N2,N2,N2,9,PAr:»,,  (9(1X,1PD13.6))  •) 
Cf»fiCff->cr.em.r»j     DEBUG     ABOVE 

CALL    BALANC  (N2,  N2,PA,  LOW  ,  I  H  IGH,  D1) 
CALL    ORTHPS  (N2,  N2,  LOW  ,IHI3tf  ,  FA,  D2) 
CALL    ORTRAM  ?N2. N2. LOW ,IHI3H.FA,D2,X) 
CALL    HQF2(N^,  N5 , LOW. I  HIGH. F A, WR , HI,  X,  I  ERR) 
Ir(IEPP     .NE.     0)      CO    TO     1000 
CALL    BALBAK  (N2,  H2#  LOW  .IHI3H-D1,  N2,X) 
q  i»ter»i»»rrjuri»i»re(i  i»ntoC)i',»  rs»i  ■%«  *b$  m>*  ri*at-G*> 

CALL    PAFFNT(N2,N2,N2.9,X,4,  •  (9(1X,1PD13.6))  •) 
CO»ntt")pc«rr-»f.     Dr-BUG    ABOVE 

100    CONTINUE 
C  ::::::::::     DETERMINE    MODAL    MATRICES 

IF(IYU    .EQ.     1)     GO    TO    130 
C  ::::::::::     HSUBU 

DO    110    I=1,sc 

DO    1 10    J=1 , H2 

ST    =    0.D0 

DO    105    K=1 , NS 
105    ST    =    ST    -    C(I  ,K)  »X  (K,  J) 
110    HU(I.J)      =    Si 

GO    T6     150 

C  ::::::::::     HSUBY 

130    DO    1U0    7=1  ,NO 

DO    1U0    J=1, N2 

ST    =    0.  DO 

DO    135    K=1,NS 
135    ST    =     ST    *    H  (I  ,K)  »X  (K,  J)     -    H  (I ,  K)  *  X  (  NS  ♦  K  ,  J) 
1U0    HY(I,J)     =    s. 

CALL    PAFPNT  (NO, NO, N2, 9, HI, 4  ,  •  (9  ( 1  X, 1 PD1 3. 6) )  ' ) 
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150 

c 


CALL 
CALL 


155 
160 

c 

200 


210 


IF  (IN 

if  (in 


E.I  AX 

DO    21 

EM3D 

IF(EM 

CONTI 

EMOD 

EMOD  = 


212 
216 

220 


218 


»Ct:*3(k    DEBOG    ABOVE 
BINV(NSQ.X,N2,STfD1,D2 
RAPRNT  (N2,N2,N2,9,X,4, 
*r-.:~.m    DEBOG    ABOVE 

::::::::::    GSUBH 

DO    160   1=1, N2 

DO     160    J  =  1  ,NG 

ST   =    0.  ono 

DO    155    K  =  1,NS 

ST    =    ST    -    X  f!,MS*K) ->GAM(K,J 

GW(I,J)     =    ST 

CALL    RAPRNT  (N2.N2,  NG,  9,  3W,  !» 
W:Mi?*ftui¥li9    DEBUG    ABOVE 

:::  ::    USE   SELECTED    BOB 

ORM    .LE.     NG)     DNORM    =    1 

ORtt    .GT.     NG)     DNORM    =    1 

:::::     DETERMINE    BANOWI 

=    0.D0 

0    1=1 .  N2 

=    DABS  (WR(I)  »?2    +  WI  (I) 

OD    .GT.     EM  AX)     EH  AX    =    E 

NUS 

=    DSORT(EMAX) 

2»SMOD 

:: : ::     ROUND    UP   TO  NEAR 

=    DLOGIOfEHOD) 

OG     .LT.     0. DO)      TPOW    =    - 

00     .GE.     0.  DO)      IPOW    = 

=    EMOD^IOef  f-IPOW) 

AX    .GT.     2.  DO)     EMOD    =    2 

AX     .GT.     U.  DO        EMOD    =    U 

AX    .GT.     5. DO i     EMOD    =    5 

AX    .GT.     8. DO i      EMOD    =    8 

AX     .GE.     10. DO)     EMOD    = 

=  EMODa10»»IPOW 

EMAX/20. DO 

:: : ::     ADD    10    POINTS    3 

OD    .LT.    5.0)     GO    TO    212 

=    1.0D1 

3 

216 
=    5. DO 
2 

NUE 

:: :  ::    STORE    30    FREQUEN 
0    1=1,20 
=    DV*  (1-1) 
8   1=1,3 
=    20    ♦    3»(I-1) 
218    J=1,3 

X    =    MOD     (IK»J-1,  3)     ♦    1 
J    =    0 

FflK  -EQ.  2  .A NO.  J  .3 
(IP*J)  =  DW1  (IX)  £.i0?->  ( 
NUE 
MOD  (IK, 3 


(9(  1X,1PD13.6) )  •) 


,'  (9(1X,1PD13.6))  •) 

MALIZATION 

.DO/0 (IKORH, INOR5) 

. DO/R (IN3RH-NG,IN0RM-NG) 

DTH    OF    CONTROLLED    SYS 


ELOG 

IF(EL 

IF(EL 

EMAX 

IF(EM 

IFfEB 

if(eh 

IF  (  EM 

IF(EK 
EMAX 

DW    = 


IP(SM 
EMAX 
IK  = 
GO  TO 
EMAX 
IK  = 
CONTI 


DO     2  2 

Mrii 

ip 

DO 

I 

J 

I 

W 

CONTI 

IX    = 

W(30) 


i(IK,3)     ♦    1 
DW1  (IX)  •*,\0»»  I  IPOW*  3 
::     LARGE    LOOP    THRU 


cm  2) 

MOD 


) 

EST    2, U, 5,8,  10 

IPINT (DABS (ELOG) 
IDINT  (EL03) 

.DO 

.DO 
.DO 
.DO 
10.00 


DECADES    UP 


CIES 


E.    2)     JJ  =  1 

IP0W*I-1*JJ*IK-2) 


♦  IK-2) 

OUTPUTS 


T) 


250 


IF(IY 

I?(IT 

DO    UO 

DO 

FS 


NO 
'    NC 


DO 


IFf  IT 
8020    POBHA 

V    r.z 

IFfIT 

8030    FORMA 

1*     CON 


U  .  FO.  1)  NL 
U  .  EQ  .  2  j  N  L 
0    L  =  1,NL 

250  1=1,30 
D(I)  =  0.D0 
:::::     LOOP    THRU    PROCESS    NOISE 

3J0    1=1. NG 
DN1    =     DN6RH*Q(I,I) 
0    .EQ.     1     .AND.     IFT    -ED 
T(/'     TRANSFER     FUNCTION 
ASUPSMENT     ',12) 
U    .SO.     2     .AND.     IPT    .S3 
?(/«     TPANSFER     FUNCTION 
TnOL    '  ,12) 


.     1)      WRITS  (6,  8020)     I,L 
F?OM    PROCESS     NOISC    ' ,12,'     TO' 

.     1)     WRITE f6, 8030)     I.L 
FROM    PROCESS     NOISE    ',12,'     TO', 
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IF(IYO.EQ.  1)  CA 

1  RES,B3,CC.IPT 

IFflY  O.SQ.  2)  CA 

1  PES.  BB.CC^PT 

DO    280    K=1,20 

ZZ    =    DCMPLX  ( 

Of.    =    H  (K) 

DO    260    11=1, 

IF(WI(II)[ 

25U  ZD    =    DC-PL 

ZZ    =    RESri 

GO    TO    260 

256  a  E    =    W  R ( 1 1 

AI    =    WI (II 

ZD    =    DCMPL 

ZN    =    DCMPL 

z  z  =   z  z  ♦ 

260  CONTINUE 

280  PSD(K)     =    PSD 

300      CONTINUE 
C  ::::::::::     GSUBV 

DO    320    1=1, N2 
DO    320    J=1 ,NO 
ST    =    0. DO 
DO    315    K=1,NS 
315    ST    =    ST    ♦    X  (I,K)  &KS 
320    GV(I,JI     =    ST 

CALL  EAPRNT  (N2,  N2,  N 
CCfrOf'CwnjoaconiMiCi  DE3UG  A 
C  ::::::::::     LOOP    THR 

DO    390    1=1 .NO 

DN1    =    DNOBK*R(X,X 
IFflYU    . EQ.     1     .AND. 
8040    FOF-IATf/'     TRANSFER 
1     •     TO    MEASUREMENT     ' 
IF(IYO    .EQ.     2     .AND. 
8050    FOFMATf/'     TRANSFER 
1    '    TO    CONTROL    *  .  12) 
IF  (IYU.ZQ.  1JCALL 
1  BE,CC,I?T) 

IF(IYU.E0-2)CALL 
1  BB.CC,IPT) 

DO    380    K=1, 30 

ZZ    =    DCMPLX (0. D 
OH    =    H  (Kl 
DO    360     11=1, N2 
IF(WIfll)}      36 
35U  ZD    =     TC.IPLXf- 

ZZ    =     ZZ    ♦     RES 
GO    TO    360 
356  RE=WR  (II) 

AI    =     WI  (II) 
ZD    =     DCMPLX  (R 
ZN    =     DCMPLX     R 
ZZ    =     ZZ    ♦     ZN/ 
360  CONTINUE 

IFflTO    .EQ.     2    .OR. 
PSD  (M     *     PSD(K) 
378  PSD(K)     =     rSD(K)     ♦ 

380         CONTINUF 
390  CONTINUE 

IPflYU    .EQ.     1)     «RIT 
IFJIYO    .E3.     2)     WRIT 
9000    FOHMATf/'     PSD    OF    OU 
1       ,  'NORMALIZED    PSD) 
9010    FORMAT(/'     PSD    OF    CO 
1       ,  ■  NORrALIZED    PSD) 
HRITE  {6,9020)      (W(I) 
9020    PQrIAT  (4  (IX,4   ('  ,  -11 
400    CC5TINUE 
R  ET  G  B  H 
10  00    CONTINUE 


LL    RESID  (I,L,N2,J:c?  .  ■„ 

LL    RESID 

) 


(I,L,N2,JC? 
0.  D0,0.  DO) 


N2 

260, 254 
X(-WR  (II 
I)  /ZD    ♦ 


G,GH,NL, HY,WE,HI, 
•  ','S,GW,NL,HU,HR,WT, 


X(EE>'>2 
X(RES  (II 

zr;/ZD 


,256 
^OM-WI(IT)    , 


♦      AIf*2     -       ^M.3,  Mtcttniii 

'     Ai    ""-  (  -  •)  *RE,RES (II) «Oh) 


(K)     ♦    DN  1*  (ZZ*DC?S  Sfi     ZZ)  ) 


GE  (K, J) 


♦    X{I,HS*X)   »rBGS(K,J| 
.'  (9(1X,1P'.M  ._6))  ,j 

U     KEAS    NOISE 


0,  9,GV,'4 

BOVE 


) 

IPT    .  EO 
FUNCTION 

'I2L 
IPT    .E3 

FUNCTION 

SESID  (I, 
RESID  (I, 

0,  0.D0) 


II      HRlTTif. 


'PROM    Siis^?!gS?>.Jiif 

L.N2,JCr,K^,  4:v,NL  ,HI ,  If  R  , HI, RES, 

L'N2'JCI,'S0"~V,NL,HU,HR,WI,RES, 


0,  354,356 

HP  (Ilf  ,  DM-WI  (II)  ) 

(IT)/ZD 


E»«.2  ♦  A 
ES  (II*1| 
ZD 

I     .NE.     L 
♦     DN1 
DN1»  (ZZ 


E(6,9033 
e]6,9013 
TTUT' ,13 

''I 

srr.oL',  I 

-   *•    9         9  9    & 


1*^2    -on>»?    _2    no^rF'kn-i 


)     GO    TO    378 
"DCONJG (ZZ) j 


FORCED     bT     ALL    soiSE-(PAD    PPE3,  • 
POfcCRD    BY    ALL    NOISE-  (RAD    F5E3,' 


1-1.30) 
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CALL    EREXIT  (N2,PA,IEFR) 

RETURN 
END 


SUBROUTINE     ER  EX  IT  (N  ,  k  ,  IERR) 

!  EEEXIT    RETURNS    THE     NUNEER     Or    THE    EIGENVALUE    WHERE    HQR2 

:  FAILS,    THEN    STOPS    THE    PROGRAM. 

INTEGER    IERR 
DOUBLE    PRECISION    A 
DIMENSION    A(N,N) 
WRITE  (6,9000)     IERR 
9000    FORMATf'     FAILURE    IN    HQR2    ON     EIGENVALUE    NO.     •  ,I3> 
CALL    RAP EST (N ,N  ,N ,9  ,  A  ,4 ,  ■  (9  (1X,  1 PD1 3. 6)  )  • ) 
STOP 
END 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 


c 


c 


C  SENSITIVITY    COVARIANCE    PECSEla  c 

C  THIS     PROGRAM    IS    OSED    TO    SOLVE    THE    E85  0-     SP^ttisttv  r 

C  EQUATIONS    WHEN    THERE     IS    AN     INCORRECT     I-slE^Imtattoh  r 

C  OF    DYNAMICS    IN    THE     DESIGN    DF    THE    Kil'lV     FnTFP       kp  r 

C  EQUATIONS    BECOME  '      FILTER. THE  C 

C  PDOT*  (F*-K*H)  P*P{F*-K*H)  T*DFV*VT3Fr*303T*K*RKftT  r 

C  VD0T=FV  +  V(F-N-1PH)  T*UDFT-GQ3T  r 

C  UD3T=FU*UFT*GQGT  q 

C  THE     PRINCIPAL    PROGRAM    INPUTS    ARE    THE     FOMOWTNr    ~n-  r 

C  LLECTION    OF    SYSTEM     AND    FILTER    MATRICES  c 

C  PO  THE    INITIAL    COVARIANCE     MATRIX  (NXV)  r 

C  F  THE    TRUTH    MODEL    DYNAMICS    MATRIX'  NX  Nl  r 

C  F*  THE    FILTER    MODEL    DYNAMICS    MAT8II IK  t{i»  r 

C  H  THE    TRUTH    MODEL    MEASUREMENT    MA  T?  t1y  ,  VU.,     UH=.RP  r 

C  L    IS    THE    MEASUREMENT    VECTOR    DIMENSION  r 

C  GQGT  THE    INPUT    NOISE    COVARIANCE    MArRZXfNXl  r 

C  R  THE    MEASUREMENT     NOISE    COVARIANCr     JiTSu/in>  r 

C  Kf  FILTER    GAIN(NXL)  ""  ulUM  U 

C 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

c 

C  THIS     PROGRAM  HAS    BEEN     DEVELOPED    USIKT.     -hp    tksi     -tbripy 

C  AVAILABLE    IN  THE    COKPUTEE    CENTER    OF    TH^'sAVAI        "1Dn*t<I 

C  POSTGRADUATE  SCHOOL  "       n,ftl- 

C 

c 

IMPLICIT    REAL'S     (A-H.O-Z) 

COMMON    F  (7,7)  ,FSJ7,7)  ,GQGT(7)  ,AK  (7.  21  ,P  (2    2\ 
*AKR  KT  Q.I)  ,  Or  (7    1)  ,  FT  (7,7)  J  FSKKH?  ( 7  .  V  f  ,  D  FT  ( V  !  7>  . 
WFSMKH  (7,7j  ,H(2,7>  '  l    '     '' 

COMMON/KTR/N, NS,MPD 

DIMENSION    PFULL  (7,7) , PS3R(7) 

DIMENSION    DDT  (7) 

DIMENSION    U(26),V(7,7 


C 


"VAR  (105) , 
[MANSION 


OK    "("l.V    7,7     ,P    23     .OOI2fl|.vD/7,7)f 
),DRVmS5)     C(2U),WK(  105,3)  ,PD(23)  '     I  ' 
ON    TMP^I  (7,7)  {:MP2  (7,7)     TMP5(7,7) 
ENCE     (Uh[,VAR  (1)  )  .  IVjl .  1)      /AR  ft  91  )  .  (P  fit 


PI! 

EQUI  V  ALi 

*VAR  (78) 

"DRV  (7  8) 
C 

C  N=ORDER    OF    THE    SYSTEM    MODEL 

C 

C  NP=NUHBEF    OF    POINTS 

C 

C  NPD=CONTPOL    OF    INITIAL    DIAGNOSTIC    OUTPUT 

C 

C  DT=TIME    INTERVAL 

C 

EXTFRNAL    FUN 
CALL    UGET10  (3,5,6) 
C 
C 

C  THE     FOLLOWING    SECTION     READS     THE    SPEClPl^n    INPUT 

C  MATRICES, F,F>, GQGT, K-H    AND     R  ~u  ul 

C 
C 


READ  (5.9P1  N,  NP,NPD,  DT 
MATJ JT5 ,F10. " 
TE  (bf97f 'i 

<V  (N*1) /2 
jiS»N^-2*1 

NV=2^NS*K*«2 


98     FORMAT(3T3 .F10. 5) 

WRITE  (b-97)  'i,  S?  .NPD.DT 
FO?MAT('  1'  ,315.  lX.Glo-1 
r  (N*1)  /2 

;  ♦«.•'- *2  ♦  i 


97    FO°r.AT(T  V  ,315,  1X,G^6.10) 
NS=N»'' 
S  1  *  S  S  • 


99     PC?MAT(8Fia.S) 
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DO 

1  REA 
CAL 

DO 

2  HEA 
CAL 
REA 
CAL 

DO 

3  REA 
CAL 

DO 

M  REA 
CAL 

DO 

5  REA 
CAL 

DO 

6  VAR 
DO 
DO 

7  DF( 

CAL 
CAL 
CAL 
CAL 

CAL 

I  HP 

DO 

DO 
DFT 
2  0  FT( 
CAL 
CAL 
CAL 
CAL 
CAL 


1     1=1,  N 


D(5,9§j      <?  (I.J)  ,J=1,N) 
L    OSKFH  ('F'  ,1  ,P,7,  N,N,1) 


2    1=1, N 


0(5, 9§)      (PS(I,J),J  =  1,N| 

L    USWFM ('FS'  ,2, FS,7,N,N,  1) 

0(5,99)      (GQG* if}  .1-1.11 

L    OSWFV  ('GQGT*  ,l|,GQGT,N,  1,1) 


3    1=1, N 


D(5,99l      (AK(I,  J)  ,J  =  1,2) 
L    USWFH  ('K'  ,  1,  AK,7,N,  2,  1  ) 

u   1=1.2 

D(5.0§)      (H  (I.J)  ,J=1,N) 
L    USWFM  ('H'  ,1,H,2,  2,N,1| 


5    1=1,2 


D(5,9§)      (R  (I.J),J=1,2) 
L    US'JFH  ('H'  ,  1,R, 2,  2,2, 1| 


6  1=1,105 

7  J=1,N 


R) 


COLATE    THE    DIFP2PENCE    BETWEEN    THE    DYKAHICS 
LEHENTED    IN    THE    FILTER    AND    THE    PLANT,     DF=F^-P 


20    1=1, N 
20    J=1, N 

I.J)  =6f  (I.J) 


(I.J)  =  DF(I    J) 

t,J)=F(I,Jj 

L    VTF.AtU  fDFT,M,M,7) 

L    QSWFB ('DEL    FT'  .6  ,DFT,7  ,N,N,  1) 

L    VTFANX jFT.N.N.7) 

L    USWFH  ('PT',2.?T, 7,N,N,   1) 

L    VflULFP  (AK,H,N,2,  N,7,2,  TMP1 ,7,IER) 


DO    21    1=1,7 
DO    21    J=1,7 
FSM 
21    FC.1 
CAL 
CAL 
CAL 
T=0. 
TOL=1.D-5 
I  HO  =  1 
L=0 
IF(N.  EQ.7)     GO    TO    11 


KH  (I.jf  =FS  (I,  J)  -T.1P1  (I,  J) 

KHT(I.J)  =  K5flKH(I  .J) 

L    USWP3  ('PS-KH',  5,  FSflK",  7,N,  N,  1) 

L    VTKAM*  jPS.".K.HT,N,  N.7) 

L    OSWFfl  ('(FS-KH)  T1  ,  8,  F3.1  KHT,  7  ,  N  ,  N,  V 


DO 
L  =  L 

31  VAR 

DO 
DO 
L^L 

32  VAR 

DO 

L  =  L 

33  VAR 
1  1    DO 


31  1=1, NS 
(L)=U  (I) 

32  1  =  1, H 

32  J  =  1,N 
♦  1 
(L)=V(I,J) 

33  1=1, fl 
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TEN 

TEN 

DVE 
DIF 

CAL 

IF( 
CAL 


D=FINAL    TIME 

D=K*DT 

RK    SUBROUTINE    FINDS    THE     SOLUTION    OF     THE    SYSTEM    OF 
FERENTIAL    EQUATIONS 

L    DVERK  (NV,?UK,T,V AS, TEN D,TOL , IND,  C,  10  5,HK,IER) 
IND.LE.  O.OR.IER.  NE.O)  STOP 
L    VCVTSP  (VAR  (N1)  ,N,PFULL  ,7) 


CALCULATE    AND    PRINT    THE    RMS     ESTIMATE    EPRORS 


DO 

REA 

PFU 

30    PS2 
WRI 

9  0    FOR 

IF 
CAL 
CAL 
CAL 
1  0    CON 


30    1  =  1,  N 
F=PFULL  (1,1 


F  =  PFULL  (1,1) 

LL  (1,1)  =  DA3S  (REAP) 

R  (I)  =DSQRr(?FULL  (I  ,1)  ) 

TEJ6,  90)T,  JPSQR(I)  ,1=  1  f  N  ) 

MAT  ('0?=',n0.5,  •     PSR=»  ,  7G15  .7) 

DESIRED    PRINT    THE       COVARIANCE     MATRICES, P,U    AND    7 

L    USWSM  (,0,,1  ,U.  N,  2) 

L    DSWFM  (•¥■  ,  1,  VAR(NS*  1)  ,  7,N,N,2) 

L    USVSK  ('P',  1,VAR(N1)  ,N,  2) 

TINUE 
STOP 
END 

SUBROUTINE    VTRANX (A, S  .NC.IA) 
IMPLICIT    REALMS     (A-H,0-Z) 
DIMENSION    A(1A,IA) ,B(7,7J 

DO  1  1=1, N 
DO  1  J=1,N 
E(I,J)=A(J,I) 

DO  2  1=1 ,N 
DO  2  J=1,N 
A(I,Jj=B  (I, J) 

RETURN 

END 

SUBROUTINE    FUN(NV,T,V AR, DR?) 

FCN     SUBPOUTINE    IS    USED    FOR     EVALUATING     FUNCTIONS ( I NPUT) 


MPLICIT    REALMS     (A-H.O-Z) 

CMMON  F(7,7^,FSJ7,7f  ,r,oqT(7)  AK  (7  2)   P  (2,2)  , 

KRKT  (7,7)  ,  D?  (7,7)  ,  FT  (7,7)  ,  PSKK HT  (7 , 7J  ,  DPT  (7,7)  , 

SMKH    7    7|  ,H(2,7j 

OMMON/KTR/N.  NS,NPD 


J/KTR/N, 

;ion  u  (2 


DIMENSION  U(2  8).V(7,7),P(2B).UD(28),»D( 
VAB  (105)  .DRVMOi)  .C  (2**[  ;k  (  105,  9)cp5(28 
DIMENSION    TMP1  (7,7)  ,TKP2(7,  7)  ,  IMP  3(7,  7) 


»DJ7,7) , 
"    ) 


L=0 

DO     1     1=1, NS 

L  =  L*1 

1  U(I)=VAP(L) 

DO  2  1=1, N 
DO  2  J=1,N 
L=L*1 

2  V  (I  ,J)=VAR  (L) 

DO    3    1-1, RS 

L  =  L*1 

P(I)  =VAP  (L) 

"     0)  KT=NPD 


IF"(T.  EQ. 
KT=K?*1 


IP     (KT-GF.  5)     GO    TO    15 
•JRITE  (6.99)     T 
99    FORKAT(*0T=,,D25.  15) 
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15 


9 

10 


1  1 


12 


13 


CALL    OSWSB  CU',  1  ,0,  N,  2) 

CALL    OSWFM  (  »V  ,  1  ,V,  7,  N,N,2) 

CALL    USWSB  ?'P«     1,V,  N.2) 

CALL    VBULPS fF,0,H,H,7,THP1 . 7) 

IF(KT.LT.5>     CALL    USWFM (• FO ■ , 2 ,T BP1 . 7 , N , S , 2) 

CALL    VBULSF (U.N.PT.N. 7,TBP2 .7) 

I?(KT.LT.5)     CALL    OS  WF  B  ( '  OFT  •  ,  3,  TBP2  ,  7  ,  N  ,  N  ,  2 


DO 
DO 


1=1 
J-1 


X,jf=THP1  (I,  J)  +TBP2 
T.-.P  1  (I,li=TBP1  jl,lj  *C 


hJ) 


QGT 
CALL    VCVTFS  (T  ME*  1  ,  N  .  7,  UD) 
IF(KT.LT.S)     CALL    US WS H 7'  ODD T'    «  ,  OD, N, 2) 
CALL    V«ULFF{F,V,H,  N,N,7.7rTHP1,  7-IERJ 

IF(KT.LT.5)     CALL    US  KF  fl  (*  FV  •      2  .7  B  P  1 ,  7  .  N  ,  N  .  2) 

CALL    VBULFF (V. FSBKHT, N,N,N,7f7,TBP2,7,IER) 

IF(KT.LT.5J     CALL    USWF  B  ('  V>  (  FS-K  H)  T'  ,  1  0  ,  IHP  2  ,  7  ,  N,  N  ,  2) 

CALL    VBULSF(U,N  ,DPT  ,N  ,7,  TBP  3  ,  7> 

IF(KT.LT.5)     CALL    US  WF & ( » 0* D FT' ,  5,  TBP3  ,  7 , N, N , 2) 

DO 

DO 
VD 
VD 
IF 


a.rapi 


2) 
7,N,N,2) 


>)     CALL    USWFB('  P(PS-KH)  T'     9.  TSP2,7,N,N, 

:F  (DF-  V,N,  N,  N,7_7,  TBP3,7,IER) 

i)     CALL    USWFB  (•  DfW' ,4  ,TBP3,  7,N,N,  2) 


7    1=1,  N 
6    J=1.N 

'I,  J)  =TBP1  (I,  J)  ♦TBP2(I,J)  «-TBP3  (I,  J» 
1,1)  =VD  (I,lf-GQGT(I) 
;KT-LT.5)     CALL    USWFK  ('  VDDT'  ,<i  ,VD,7,  N,  H, 
CALL    VBULFS (FSBKH.P, N,N,7, T3P1 ,7) 
IFfKT.LT.5f    CALL    US WF fi ( • (FS -KH» P •     I 
CALL    VBULSP  (P,N,FSBKHT,N,7.  TBP2,7) 
IFfKT.LT.S)     CALL    USWFB('  P(P  S-KH)  TV 
CALL    VBULFF  (DF.  V,N.  N,  N,7.7.  T.1P3.7.. 
IF(KT.LT.S)     CALL    OS  WFB  (•  DP*  V»  ,»  ,TH] 

DO    8    1=1,  N 

DO    8    J=1  ,N 

TBP  1(I.J)=TBP1fI,J)  *TBP2  (I,  J)  »TBP3  (I.  J) 

CALL     VBULFB  (V,DF,N.  N,  N,7,7     TBP3,7,rEft) 

IF(KT.LT.5)     CALL    US WF B ( • Vr* DF ■  ,  5 , TBP3 , 7 , N  ,  N  ,  2) 

DO    10    1=1,  M 

DO    9    J=1,N 

TBP3  (I,J)=TBP  1  (I,  J)  ♦THP3II,  J)  ♦AKRKr{I,J) 
TBP3JI/I=TBP3(I,lWsr;srjI) 

IP(KT.LT.S»     CALL    U5WFR  (•  PODT*  ,H  ,THP3,  7  ,  H,  »,2) 

CALL    VCVTFS  (TBF3,  N.  7.  PD) 

IFfKT.LT.5)     CALL    US  WF  V  (4  POD  T  (SIT  B)  •  ,  9,  PD  ,  N  ,  1  ,  2) 

DO     11     1=1, NS 


2) 


DO  1 
L  =  L» 

DRV  ( 

DO  1 
DO     1 

L=L* 
DRV  ( 

DO  1 
L  =  L* 
DPV  ( 
IPfK 
FETU 
END 


1  1=1, NS 
L)  =UD(I) 

2  1-1,11 

2  J=1,N 
1 
L)=VD(I,J) 

3  1=1, NS 
1 

L) =PD{I) 

T.LT.5)     CALL    OS WF V ( ' S3 V • , 3  ,DP V, NV ,  1 , 2) 

RN 
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ccccccccccccccccccccccccccccccccccccccccccccccccccccccccccc 

c  c 

C  SENSITIVITY    COVARIANCE    PROGRAM  C 

c  c 

C  THIS     PROGRAM    IS    USED    TO    SOLVE    THE    ER30F.     SENSITIVITY  C 

C  EQUATIONS    WHEN    THERE    IS    AN     INCORRECT    I HPL2HENTAI ION  C 

C  OF    DYNAMICS    IN    THE     DESIGN    3F    THE    KALMAN     FILTER. THE  C 

C  EQUATIONS    BECO.IE  C 

C  C 


C  PD3T  =  (F*-K*H)  P*P(P"-KCH)  T*  D  FV«-VTDPT  +  GQGT*  IC  RK«T  C 

C  VDOT=FV  +  V{Ffi-K»H)  T  +  UDFT-GQGT  C 

C  UD0T=  FU*UPT*GQGT  C 


C  C 

C  THE    PRINCIPAL    PROGRAM    INPUTS    ARE    THE    FOLLOWING    CO-          C 

C  LLECTION    OF    SYSTEM    AND    FILTER    MATRICES                                            C 

C  C 


C  PO  THE    INITIAL    COVARIANCE     MATRIX(NXN)  C 

IXN) 

xf 

:  (LX! 

C  L    IS    THE    MEASUREMENT    VECTOR     DIMENSION    '  C 


C  F  THE    TRUTH    MODEL    DYNAMICS    MATRIX (NXN)  C 

(NXN) 
C  H  THE    TRUTH    MODEL    MEASUREMENT    MATS  IX JLXN)  , HHER E  C 


C  Ft  THE    FILTER    MODEL    DYNAMICS    MATRIX  (NXN)  C 


C  GQGT    THE    INPUT    NOISE    COVARIANCE    MATRIX  (NX)  C 

C  R  THE    MEASUREMENT     NOISE     COVARIANCE    MATRIX  (LXL)  C 

C  K*  FILTER    GAIN  (NXL)  C 

C  C 

CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 

C 

C 

C  THIS     PROGRAM    HAS    BEEN     DEVELOPED    USING    THE    IMSL    LI3RARY 

C  AVAILABLE    IN    THE    COMPUTER    CENTER    OP    THE     NAVAL 

C  POSTGRADUATE    SCHOOL 

C 

C 

IMPLICIT    REAL*8     (A-H.O-Z) 


COMMON    P  (8,81,PS]8,8)  ,GQGT(8)  ,AK(8,3)  ,5  (3.31, 
rtAKRKT  (8,8^  ,  DF  (8.8)  ,  FT  (8,8)  ,  FSMKHT( 3,3)  ,DFT  (8,8) 
*FSMKH  (8,8)  ,  H  (  3.  8) 


COM  MON/KTR/N,  n£  ,  NPD 

DIMENSION  FFULL  (8,8)  , PS2R(8) 

DIMENSION    DPT  (8) 

DIMENSION    U(36).V(8,3),P(36).UD<36I,VD(8,S)  , 
<-VAP  (136)  ,087(136)  tC(2Hi','M'J6.9),?D(36) 

DIMENSION    THP1  (8,8)  ,  7MP2  (8,  8)  .TMP3  (8.  3) 

EQUIVALENCE     {  U  (  1 j  .  VXB  Ml  )  ,  f  V  (\  ,  1 )  ,  VAR  (  3  7)  )  ,  IP  ( 1 ) 
*VAR  (101)  I,  (UD(f)  ,  &RV(1))  ,  (VD(1,  1)  ,DRV  (37)  )  ,  (PD(1)  , 
CDRV  (101) 


C 
C 

c 

c 

C 
C 

c 

C 

c 
c 

N=ORDER    OP    THE    SYSTEM    MODEL 

NP*NUHBEP    OF    POINTS 

NPD=CONTROL    OF    INITIAL    DIAGNOSTIC    OUTPUT 

DT=TIHE     INTERVAL 

EXTERNAL    PUN 

CALL    UGETIO  (3,5,6) 

c 
c 

c 
c 

THE     FOLLOWING    SECTION     READS     THE    SPECIFIE 
MATRICES, F,F», GQGT,  K*»H    AND     R 

c 

98 
97 

READ  (5,  9  8)  N.  NT,  NPD,  DT 
FORHAT(3I5  .  "10.  5) 
WFITE(6,97fN,K?.NPD.DT 
FOR  B »T  { '  1  •  , 3 1 5, IX , G 16 . 10) 

INPUT 


NV=2^  NS*  K«2 
99    FORMAT(8F10.5) 


99 


20 


21 


31 


33 
1  1 


DO     1     1*1.  * 
READ  (5,  59)      (P  (I 
CALL    USVF.1  (•?'  , 

DO    2     I*1.S 
READ(5,99)       (?S< 
CALL    USWF1 ('PS' 


1,7.8. N.N,  1) 


x5,?5t8 fftA,  i) 

READ(5,99)      (GQGT(I)  ,I«1.S) 
CALL    OSWFV  (*GQGT'  ,  4,GQGr,N,  1,1) 


DO    3    1=1, S 


READ(5.9$)       (AK( 
CALL    USWF.4!  ('«*, 


I, J)  ,J  =  1,3) 
1,Ak,3,N,3,1) 


DO    U    1*1,3 


READ  (5,94)      (H  (I.J)  .  J=  I.N) 
CALL    GSWPfl  ('H'  ,  1,H,3,  3,N,1) 

J)  ,J=1,3 
1,6,3,3,3,1) 


DO    5    1*1.3 
READ  (5,99)       (R  (I 
CALL    USWF.1  ('R'  , 

DO    *,    t*1,  136 
VAR  (I)=0. 
DO    7    1=1,  N 
DO    7    J=  1  ,  N 


0F(I, 

CALL 

CALL 

CALL 

CALL 


J)=FS  (T.J)  ■ 

us w F.i  ('Bel 

VrtULFr  (AK    ' 
VKULFP    TttP 
USWF.t  ('X8K 


F',5,DP,9,N     M    1) 
»,».3. J. 5. 5     TH?1 .3    IER) 
1.AK,N,J,N,3,8,AKPKT,3,IER) 
T* ,<*,AKRKT,3  ,N,N,  1) 


CALCULATE    THE    D 
ir.PLEIIENTED    IN 


IPPERENCE    BETWEEN    THE    DYNAfllCS 
THE    FILTER    AND    THE    PLANT,     DF=F'»-? 


DO  20  1=1, N 
DO  20  J=1,  N 
DFT  (I.J) =6?  (I.J) 


FT(I,  J)  =F(*I,jf 

CALL    VTRANX  (OF 


C 

CALL 

CALL 

CALL    OSWFS  ('P 


USWF^  ('DhL 
VTRANX  (FT 


CALL    VHUL?! 


(AK, 


,N,N,8) 

FT'  .6 ,DPT,3  ,N,N,  1) 
N.N,  9) 

.5. FT,  8,N,N,  1) 
H,N,3,  :i,8,3,  tHPI  ,8, IER) 


DO    21    1=1,8 

DO    21    J=1,8 

FS.TKH  (I.J)  =FS  (I 

FSNKHT  (I  .J)  =FSM 

CALL    USWF3  f'FS- 

CALL    VTRAMX(FS3 

CALL    USWFM  ('  (FS 

T=0. 

TOL=1 .D-5 

IND  =  1 

L=0 

IF(N.  EQ.8)      GO    TO     11 

DO    3  1 

L=L  +  1 
VAR  (L) 


,J)  -TKP1     I,  J 

KH  (I.J) 

KH' , 5, FSHKH, 8,N,N, 1) 

KHT, S, N.8) 

-KH)T',3,?S!1  KHT,  8,  N.N,  1) 


32    V 


1=1,  HS 
=  U(D 

DO    32    1=1, M 
DO    32    J=1,N 
L=L  +  1 
VAR  (L)  =V  (I  ,  J) 


DO    33    I 

L=L  +  1 


1.N 


-^io^tt. 


DO 
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TEN 

TEN 

DVE 
DIP 

CAL 
IF( 
CAL 


D=FINAL    TIME 

D=iO»DT 

F.K    SUBROUTINE    FINDS    THE     SOLUTION    DF    THE    SYSTEM    OF 
FEEENTIAL    EQUATIONS 

L    DVERK { NV,FUN,T,VAR, TEN D ,TOL ,IND,  C , 136,WK,IEE) 
IND.LE.  O.OR.IER.  NE.O)  STOP 
L    VCVTSF  (VAR  (N1)  ,N  ,PPULL  ,8) 


CALCULATE    AND    PRINT    THE    RHS     ESTIMATE    ERRORS 


DO 
REA 
PFU 
30    ?SQ 
WRI 

9  0    FOR 

IF 
CAL 
CAL 
CAL 

10  CON 
STO 
END 
SUB 
I. IP 
DIM 

DO 
DO 
1        3  (I 

DO 

DO 
2    All 

RlT 
END 
SU3 

FCN 


30    1  =  1, N 

P=PFULL  (I.  II 

LL  (1,1)  =DABS  (REAP) 

R  (I)  =DSQRT  (PFULL  (1,1)  ) 

TE  (6.90)  T,  JPSQR  (I)  .1=1,!)) 

.-.AT(^0T=,,F1O.5,  '     PSR=' ,  8G1U.7) 


DESIRED     FRINT    THE       COVARIANCE    MATRICES, ?,U    AND    V 
L    US  WSK  (].U;,  1  ,  U,  N, 3\ 

L 

TINUE 

P 


.Oi.r.wU      rnA.ii      inc.        luvat.  ihull     n« 
USWSM ('U',  1  ,U.  N,  31 
USyFM     '  V,  1  ,VAR(NS*1)  ,  8,N,N,3) 
USD  SB  ('P'  ,  1,VAR    N1)  ,  N,  3) 


ROUTINE     VTRANX  (A. H. NC.IA) 
LICIT    REAL<=8     (A-H,0-Z) 
ENSION    A  (IA,IA)  ,B(8,8) 


1     1=1,  N 

1  J=1,N 
,J)  =  A  (J, I) 

2  1=1, N 
2    J=1,N 

GrTB(I'J) 

routine  fun(nv,t,var,dr?) 

SUBROUTINE    IS    USED    FOR     EVALUATING     PO NCTIONS { I N PUT) 


IMPLICIT    REAL*8     (A-H, O-Z) 

COMMON  F  (8,  8)  ,  PS  (8,8)  ,GQGT  (8)  ,AK  (8.  3|  ,R  (3,3)  , 
*AK.RKT  (8,8)  ,  DF  (8.  8)  ,  FT  (8,8)  ,  PSMKHT  (8,3)  ,  DPT  (8, 
^PSMKH    8,8)  ,H  (3,8) 

COMMON/KTR/N,  NS.NPD 

DIMENSION    U(5b).V(8,8)/P(3S).UD(3  6),»D(8,8)  , 
OVAR  (13b)  ,DRV(136)  .C  {2«f . MX ( 136, 9l  .P6<36) 

DIMENSION    TMP1  (8,6)  ,TMP2  (8,  8)  ,7  MP  3  (8,  8) 


8J 


L=0 

DO     1     I  =  1 , N  S 

L  =  L*1 

1  U(I)  =VAR  (L) 

DO  2  1=1,  S 
DO  2  J=1,N 
L  =  L*1 

2  V  (I  ,J)=VAfi  (L) 

DO    3     1=1, NS 
L  =  L*1 

3  P(I)=VAiML) 
IP(..  EC.  C)  kT=NTD 
KT=  KT*1 

IF     (KT.GE.5)     GO    TO     15 
WRI  Tr   (6,99)     T 
99    FORMAT('0T=',D2S. 15) 
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15 


CALL    DSWSM  (*D,,1  #0,  N,  3) 
CALL    OSWFH  (•▼•-1,7,7,  H.N 
CALL    USWSM  ('P',  1  ,V,N.  31 

CALL  VflULFS  (P.O.N.N.S.Tn 
IF(KT.LT.5l  CALL  OSWFn(' 
CALL  VflULSF  (U,N.FT,N,  8,T 
IF(KT.LT.S)     CALL    USWFH(' 


,3) 


PI.  8) 

fu«  ,3,ran,8,  .     ^  3, 

HP2 ,8)  *      *  N' J» 

UPT  »,3,T»?2,S      «t     N    3) 


DO  5 

1=1,  N 

DO  U 

J=1,N 

U  T.-.P  1 

(I,J)= 

5  TMP1 

U<d= 

:«P2 
;qgt 

CALL  "VCVTFS  (7flP1,N.7,  OD) 
IF(KT.LT.S)  CALL  ■JSWSn<« 
CALL  VRULFP (F.V .N.N.N ,8, 
IF(KT.LT.5)  CALL  USaFMr 
CALL  VMULFF  (V.FSMKHT, N,N 
CALL    OSWFMC 


I.  J) 
I) 


IF(KT.LT.5)  CALL  OSWF!W« 
CALL  VnULSF(U,N,DPT,N,8, 
IF(KT.LT.5)     CALL    OSWFM(* 


UDDT'  .a     lip    j(,    5, 

fv«  ,3,rn?i    8.  v    v    3, 

('u^Ff'r5.Tnp3_SN#N#3) 


,J)  *TMP3  (I,j) 
VDDT'  ,U 


DO    7    1=1, N 
DO    6    J=1.N 

VD(I,  J)  =T,1P1  (I,  J)  +TMP2(I 
VD/I.I  =  VD  (I,  I)  -GQGT(I) 
IF(KT.LT.5)  CALL  USWFn(' 
CALL  VflULFSfFSflKH.  P.N.N 
IF(KT-LT.5J  CALL  USWFH(« 
CALL  VMULSF  (P.N. FSflKHT,N 
IFf KT.LT.5)     CALL    USWFflC 

CALL    VHULFF  (DP,  V,  N.N.N,  8  .  o  ,  in  «,o.isri 
IF(KT.LT.5)     CALL    US  WF  H  (•  b?>  V  ,4  ,TflP3,  *»     v     N 


VDDT' ,U  ,VD,8,  n     N    t\ 
,9.  TflP1  'ft)        '      •  N'3» 

(FS-KH)  P«     8,r»*M    n     m    w    ->» 
,8.  T1P2,8f  -  1'8'N'!'.3) 

P(Fs-kh)  T*,qt -«p,   a  N   N   , 

.8.  THP3,8,iEf;r         '     '    '    ' 


3) 


3) 


(I,  j>  ♦rnp3(i..,) 
vr*DP',  S.fnp«    Q    H   „ 


9 

10 


1  1 


12 


13 


DO    8    1=1  ,N 

DO    8    J=1.N 

TflPI  (I.  J)  =  TAP  1  (I,  J)  ♦TP1P2 

CALL    VSULFfl (V,DP,N. N,  N,8, 

IF(KT.LT.5)     CALL    USWFH('V 

DO    10    1=1, N 

DO    9    J=1,N 

THP3  (I,J)=TflP1  (I,  J)  +THP3  (I,  J)  +AKBKT  (1      i, 

TKP3JI.I    =T!1P3  (I,l[  ♦  GOGTJII  *     '  J  ' 

IF(KT.LT.5»     CALL    US  WF  H  ('  PDD  T«  ,  U  ,  T.1P3.  *     u    M 

CALL    VCV7P5  (TflP3,N,8,PD)  »»»». 


3) 


3) 


I  F^  KT.LT.5)     CALl'us'wFVC  PDDT(SYH)  '  ,  9  ,  m  ,  N  ,  1  ,  3) 


DO 

11  I 

=  1,  NS 

L  =  L 

♦  1 

DRV 

(L)  = 

UD(I) 

DO 

12  I 

«1,M 

DO 

12  J 

=  1,H 

L  =  L 

+  1 

DSV 

(D  = 

VD(I,J) 

DO 

13  I 

=  1,NS 

L  =  L 

♦  1 

OP.V 

(LL= 

(KT. 

PD(I) 

LT.  5)  C 

IF 

PETUBN 

END 

3) 
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